Add multiprotocol capability and IPv6

8 files changed, 393 insertions, 12 deletions

diff --git a/src/main/java/com/lumaserv/bgp/BGPFsm.java b/src/main/java/com/lumaserv/bgp/BGPFsm.java
index b38ce6a..2cf598f 100644
--- a/src/main/java/com/lumaserv/bgp/BGPFsm.java
+++ b/src/main/java/com/lumaserv/bgp/BGPFsm.java
@@ -1,6 +1,8 @@
 package com.lumaserv.bgp;
 
+import com.lumaserv.bgp.protocol.AFI;
 import com.lumaserv.bgp.protocol.BGPPacket;
+import com.lumaserv.bgp.protocol.SAFI;
 import com.lumaserv.bgp.protocol.message.BGPNotification;
 import com.lumaserv.bgp.protocol.message.BGPOpen;
 import com.lumaserv.bgp.protocol.message.BGPUpdate;
@@ -181,11 +183,15 @@ public class BGPFsm {
                 // - completes BGP initialization
 
                 BGPSessionConfiguration config = session.getConfiguration();
+                BGPOpen.Capabilities caps = new BGPOpen.Capabilities();
+                caps.getCapabilities().add(new BGPOpen.MultiprotocolExtensionCapability(AFI.IPV4.getValue(), SAFI.UNICAST.getValue()));
+                caps.getCapabilities().add(new BGPOpen.MultiprotocolExtensionCapability(AFI.IPV6.getValue(), SAFI.UNICAST.getValue()));
                 BGPOpen request = new BGPOpen()
                     .setAsn(config.getLocalAs())
                     .setHoldTime(CONFIG_HOLD_TIME)
                     .setVersion(CONFIG_VERSION)
                     .setIdentifier(config.getLocalIdentifier());
+                request.getOptionalParameters().add(caps);
                 try {
                     session.sendOpen(request); 
                     setHoldTimer(4 * 60);
@@ -237,11 +243,15 @@ public class BGPFsm {
                 // - completes the BGP initialization,
 
                 BGPSessionConfiguration config = session.getConfiguration();
+                BGPOpen.Capabilities caps = new BGPOpen.Capabilities();
+                caps.getCapabilities().add(new BGPOpen.MultiprotocolExtensionCapability(AFI.IPV4.getValue(), SAFI.UNICAST.getValue()));
+                caps.getCapabilities().add(new BGPOpen.MultiprotocolExtensionCapability(AFI.IPV6.getValue(), SAFI.UNICAST.getValue()));
                 BGPOpen request = new BGPOpen()
                     .setAsn(config.getLocalAs())
                     .setHoldTime(CONFIG_HOLD_TIME)
                     .setVersion(CONFIG_VERSION)
                     .setIdentifier(config.getLocalIdentifier());
+                request.getOptionalParameters().add(caps);
                 try {
                     session.sendOpen(request);
                     setHoldTimer(4 * 60);
@@ -259,6 +269,7 @@ public class BGPFsm {
     class OpenSent extends State {
         @Override
         void openMsg(BGPOpen msg) {
+            System.out.println("openMsg:" + msg);
             if(session.getConfiguration().getRemoteAs() == msg.getAsn()) {
                 // - resets the DelayOpenTimer to zero,
                 // - sets the BGP ConnectRetryTimer to zero,
diff --git a/src/main/java/com/lumaserv/bgp/protocol/AFI.java b/src/main/java/com/lumaserv/bgp/protocol/AFI.java
new file mode 100644
index 0000000..1dce43b
--- /dev/null
+++ b/src/main/java/com/lumaserv/bgp/protocol/AFI.java
@@ -0,0 +1,21 @@
+package com.lumaserv.bgp.protocol;
+
+import java.util.EnumSet;
+
+import lombok.Getter;
+import lombok.AllArgsConstructor;
+
+@AllArgsConstructor
+@Getter
+public enum AFI {
+    IPV4(1, "IPv4"),
+    IPV6(2, "IPv6");
+
+    int value;
+    String desc;
+
+    public static AFI fromInteger(int value) {
+        return EnumSet.allOf(AFI.class).stream().filter(e -> e.getValue() == value).findAny().orElseThrow(
+                () -> new IllegalArgumentException("unknown address family: " + value));
+    }
+}
diff --git a/src/main/java/com/lumaserv/bgp/protocol/IPPrefix.java b/src/main/java/com/lumaserv/bgp/protocol/IPPrefix.java
index be70c4f..f68677c 100644
--- a/src/main/java/com/lumaserv/bgp/protocol/IPPrefix.java
+++ b/src/main/java/com/lumaserv/bgp/protocol/IPPrefix.java
@@ -13,7 +13,7 @@ import java.net.UnknownHostException;
 public class IPPrefix {
 
     byte[] address;
-    byte length;
+    int length;
 
     public String toString() {
         try {
diff --git a/src/main/java/com/lumaserv/bgp/protocol/SAFI.java b/src/main/java/com/lumaserv/bgp/protocol/SAFI.java
new file mode 100644
index 0000000..5fa6b99
--- /dev/null
+++ b/src/main/java/com/lumaserv/bgp/protocol/SAFI.java
@@ -0,0 +1,21 @@
+package com.lumaserv.bgp.protocol;
+
+import java.util.EnumSet;
+
+import lombok.Getter;
+import lombok.AllArgsConstructor;
+
+@AllArgsConstructor
+@Getter
+public enum SAFI {
+    UNICAST(1, "NLRI Unicast"),
+    MULTICAST(2, "NLRI Multicast");
+
+    int value;
+    String desc;
+
+    public static SAFI fromInteger(int value) {
+        return EnumSet.allOf(SAFI.class).stream().filter(e -> e.getValue() == value).findAny().orElseThrow(
+                () -> new IllegalArgumentException("unknown subsequent address family: " + value));
+    }
+}
diff --git a/src/main/java/com/lumaserv/bgp/protocol/attribute/MPReachableNLRIAttribute.java b/src/main/java/com/lumaserv/bgp/protocol/attribute/MPReachableNLRIAttribute.java
new file mode 100644
index 0000000..0cb2e49
--- /dev/null
+++ b/src/main/java/com/lumaserv/bgp/protocol/attribute/MPReachableNLRIAttribute.java
@@ -0,0 +1,65 @@
+package com.lumaserv.bgp.protocol.attribute;
+
+import java.net.InetAddress;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.Collection;
+
+import lombok.Getter;
+import lombok.Setter;
+
+import com.lumaserv.bgp.protocol.AFI;
+import com.lumaserv.bgp.protocol.IPPrefix;
+import com.lumaserv.bgp.protocol.SAFI;
+
+@Getter
+@Setter
+public class MPReachableNLRIAttribute implements PathAttribute {
+
+    AFI afi;
+    SAFI safi;
+    InetAddress nextHop;
+    Collection<IPPrefix> nlriPrefixes;
+
+    public MPReachableNLRIAttribute(byte typeCode, byte[] data) {
+        afi = AFI.fromInteger((data[0] << 8) | data[1]);
+        safi = SAFI.fromInteger(data[2]);
+        int nhLen = data[3];
+        byte[] raw = new byte[nhLen];
+        System.arraycopy(data, 4, raw, 0, nhLen);
+        try {
+            nextHop = InetAddress.getByAddress(raw);
+        } catch (UnknownHostException ex) {
+            throw new RuntimeException(ex);
+        }
+        int nlriLen = data.length - 5 - nhLen;
+        nlriPrefixes = new ArrayList<>();
+
+        int offset = 4 + nhLen + 1;
+        int offsetOffset = offset;
+        while ((offset - offsetOffset) < nlriLen) {
+            IPPrefix prefix = new IPPrefix()
+                    .setLength((int)(data[offset]&0xff)) // FIXME IPv4/IPv6 etc
+                    .setAddress(new byte[16]);
+            offset++;
+            int addressLen = (int) Math.ceil(prefix.getLength() / 8d);
+            System.arraycopy(data, offset, prefix.getAddress(), 0, addressLen);
+            offset += addressLen;
+            nlriPrefixes.add(prefix);
+        }
+    }
+
+    public byte getTypeCode() {
+        return 14;
+    }
+
+    public byte[] build() {
+        // FIXME
+        return null;
+    }
+
+    public String toString() {
+        return "{AFI:" + afi + " SAFI:" + safi + " NextHop:" + nextHop.getHostAddress() + " NLRIs:" + nlriPrefixes + "}";
+    }
+}
diff --git a/src/main/java/com/lumaserv/bgp/protocol/attribute/MPUnreachableNLRIAttribute.java b/src/main/java/com/lumaserv/bgp/protocol/attribute/MPUnreachableNLRIAttribute.java
new file mode 100644
index 0000000..32600d9
--- /dev/null
+++ b/src/main/java/com/lumaserv/bgp/protocol/attribute/MPUnreachableNLRIAttribute.java
@@ -0,0 +1,37 @@
+package com.lumaserv.bgp.protocol.attribute;
+
+import lombok.Getter;
+import lombok.Setter;
+
+import com.lumaserv.bgp.protocol.AFI;
+import com.lumaserv.bgp.protocol.SAFI;
+
+import java.nio.ByteBuffer;
+
+@Getter
+@Setter
+public class MPUnreachableNLRIAttribute implements PathAttribute {
+
+    AFI afi;
+    SAFI safi;
+    int nlriLen;
+
+    public MPUnreachableNLRIAttribute(byte typeCode, byte[] data) {
+        afi = AFI.fromInteger((data[0] << 8) | data[1]);
+        safi = SAFI.fromInteger(data[2]);
+        nlriLen = data.length - 3;
+    }
+
+    public byte getTypeCode() {
+        return 15;
+    }
+
+    public byte[] build() {
+        // FIXME
+        return null;
+    }
+
+    public String toString() {
+        return "{AFI:" + afi + " SAFI:" + safi + " NLRIs:" + nlriLen + "}";
+    }
+}
diff --git a/src/main/java/com/lumaserv/bgp/protocol/attribute/PathAttribute.java b/src/main/java/com/lumaserv/bgp/protocol/attribute/PathAttribute.java
index 4a10daa..5f4816e 100644
--- a/src/main/java/com/lumaserv/bgp/protocol/attribute/PathAttribute.java
+++ b/src/main/java/com/lumaserv/bgp/protocol/attribute/PathAttribute.java
@@ -19,6 +19,10 @@ public interface PathAttribute {
                 return new AggregatorAttribute(typeCode, data);
             case 8:
                 return new CommunitiesAttribute(typeCode, data);
+            case 14:
+                return new MPReachableNLRIAttribute(typeCode, data);
+            case 15:
+                return new MPUnreachableNLRIAttribute(typeCode, data);
             case 16:
                 return new ExtendedCommuntiesAttribute(typeCode, data);
             case 17:
diff --git a/src/main/java/com/lumaserv/bgp/protocol/message/BGPOpen.java b/src/main/java/com/lumaserv/bgp/protocol/message/BGPOpen.java
index 875f7bc..ae4e32a 100644
--- a/src/main/java/com/lumaserv/bgp/protocol/message/BGPOpen.java
+++ b/src/main/java/com/lumaserv/bgp/protocol/message/BGPOpen.java
@@ -1,9 +1,14 @@
 package com.lumaserv.bgp.protocol.message;
 
+import lombok.AllArgsConstructor;
 import lombok.Getter;
 import lombok.NoArgsConstructor;
 import lombok.Setter;
 
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
+
 @Getter
 @Setter
 @NoArgsConstructor
@@ -13,23 +18,240 @@ public class BGPOpen {
     int asn;
     int holdTime;
     byte[] identifier;
+    List<Parameter> optionalParameters = new ArrayList<>();
 
-    public BGPOpen(byte[] message) {
-        version = message[0];
-        asn = ((message[1] & 0xFF) << 8) | (message[2] & 0xFF);
-        holdTime = ((message[3] & 0xFF) << 8) | (message[4] & 0xFF);
+    @Getter
+    enum ParameterType {
+        CAPABILITIES(2);
+
+        byte type;
+
+        ParameterType(int type) {
+            this.type = (byte)type;
+        }
+    }
+
+    interface Parameter {
+        byte getType();
+        int getValue(byte[] dst, int offset);
+    }
+
+    @AllArgsConstructor
+    @Getter
+    public static class UnknownParameter implements Parameter {
+        byte type;
+        byte[] value;
+
+        @Override
+        public int getValue(byte[] dst, int offset) {
+            System.arraycopy(value, 0, dst, offset, value.length);
+            return value.length;
+        }
+
+        @Override
+        public String toString() {
+            return "Unknown{type=" + type + ", length=" + value.length + "}";
+        }
+    }
+
+    @Getter
+    @NoArgsConstructor
+    public static class Capabilities implements Parameter {
+        List<Capability> capabilities = new ArrayList<>();
+
+        @Override
+        public byte getType() { return ParameterType.CAPABILITIES.getType(); }
+
+        @Override
+        public int getValue(byte[] dst, int offset) {
+            int first = offset;
+
+            for (Capability cap: capabilities) {
+                dst[offset++] = cap.getCode();
+                int lengthPos = offset++;
+                int length = cap.getValue(dst, offset);
+                offset += length;
+                dst[lengthPos] = (byte)length;
+            }
+
+            return offset - first;
+        }
+
+        Capabilities(byte[] src, int offset, int length) {
+            int first = offset;
+            while ((offset - first) < length) {
+                byte code = src[offset++];
+                int capLength = src[offset++];
+
+                if (code == Capability.Code.MULTIPROTOCOL_EXTENSION.getCode()) {
+                    capabilities.add(new MultiprotocolExtensionCapability(src, offset, capLength));
+                } else {
+                    byte[] value = new byte[capLength];
+                    System.arraycopy(src, offset, value, 0, capLength);
+                    capabilities.add(new UnknownCapability(code, value));
+                }
+
+                offset += capLength;
+            }
+        }
+
+        @Override
+        public String toString() {
+            StringBuffer buf = new StringBuffer();
+
+            buf.append("caps:[");
+            boolean first = true;
+            for (Capability cap: capabilities) {
+                if (!first)
+                    buf.append(", ");
+                buf.append(cap);
+                first = false;
+            }
+            buf.append("]");
+            return buf.toString();
+        }
+    }
+
+    interface Capability {
+        @Getter
+        enum Code {
+            MULTIPROTOCOL_EXTENSION(1);
+
+            byte code;
+
+            Code(int code) {
+                this.code = (byte)code;
+            }
+        }
+
+        byte getCode();
+        int getValue(byte[] dst, int offset);
+    }
+
+    @AllArgsConstructor
+    @Getter
+    public static class UnknownCapability implements Capability {
+        byte code;
+        byte[] value;
+
+        @Override
+        public int getValue(byte[] dst, int offset) {
+            System.arraycopy(value, 0, dst, offset, value.length);
+            return value.length;
+        }
+
+        @Override
+        public String toString() {
+            return "Unknown{code=" + code + ", length=" + value.length + "}";
+        }
+    }
+
+    @AllArgsConstructor
+    public static class MultiprotocolExtensionCapability implements Capability {
+        int afi;
+        int safi;
+
+        @Override
+        public byte getCode() { return Capability.Code.MULTIPROTOCOL_EXTENSION.getCode(); }
+
+        @Override
+        public int getValue(byte[] dst, int offset) {
+            dst[offset] = (byte)(afi >> 8);
+            dst[offset + 1] = (byte)(afi & 0xFF);
+            dst[offset + 2] = (byte)0;
+            dst[offset + 3] = (byte)safi;
+            return 4;
+        }
+
+        MultiprotocolExtensionCapability(byte[] src, int offset, int length) {
+            if (length != 4) {
+                throw new RuntimeException("Bad MultiprotocolExtensionCapability length");
+            }
+
+            afi = ((src[offset] & 0xFF) << 8) | (src[offset + 1] & 0xFF);
+            safi = src[offset + 3] & 0xFF;
+        }
+
+        @Override
+        public String toString() {
+            return "{afi:" + afi + ", safi:" + safi + "}";
+        }
+    }
+
+    public BGPOpen(ByteBuffer message) {
+System.out.println("BGPOpen pos:" + message.position());
+        version = message.get();
+        asn = message.getShort() & 0xFFFF;
+        holdTime = message.getShort() & 0xFFFF;
         identifier = new byte[4];
+        message.get(identifier);
+        int optLength = message.get();
+        int optStart = message.position();
+
+        while ((message.position() - optStart) < optLength) {
+            byte type = message.get();
+            byte length = message.get();
+            byte[] array = message.array();
+            int offset = message.position();
+
+            if (type == ParameterType.CAPABILITIES.getType()) {
+                optionalParameters.add(new Capabilities(array, offset, length));
+            } else {
+                byte[] value = new byte[length];
+                System.arraycopy(array, offset, value, 0, length);
+                optionalParameters.add(new UnknownParameter(type, value));
+            }
+
+            message.position(offset + length);
+        }
+    }
+
+    public BGPOpen(byte[] message) {
+        this(ByteBuffer.wrap(message));
     }
 
     public byte[] build() {
-        byte[] message = new byte[6 + identifier.length];
-        message[0] = version;
-        message[1] = (byte) (asn >> 8);
-        message[2] = (byte) (asn & 0xFF);
-        message[3] = (byte) (holdTime >> 8);
-        message[4] = (byte) (holdTime & 0xFF);
-        System.arraycopy(identifier, 0, message, 5, identifier.length);
+        ByteBuffer buf = ByteBuffer.allocate(256);
+        build(buf);
+        byte[] message = new byte[buf.position()];
+        buf.rewind();
+        buf.get(message);
         return message;
     }
 
+    public void build(ByteBuffer b) {
+        b.put(version);
+        b.putShort((short)asn);
+        b.putShort((short)holdTime);
+        b.put(identifier);
+
+        byte[] array = b.array();
+        int optLengthPos = b.position();
+        b.put((byte)0);
+        for (Parameter parameter: optionalParameters) {
+            b.put(parameter.getType());
+            int lengthPos = b.position();
+            b.put((byte)0);
+            int valuePos = b.position();
+            int length = parameter.getValue(array, valuePos);
+            b.position(valuePos + length);
+            b.put(lengthPos, (byte)length);
+        }
+        b.put(optLengthPos, (byte)(b.position() - optLengthPos - 1));
+    }
+
+    @Override
+    public String toString() {
+        StringBuffer buf = new StringBuffer();
+
+        boolean first = true;
+        for (Parameter parameter: optionalParameters) {
+            if (!first)
+                buf.append(", ");
+            buf.append(parameter);
+            first = false;
+        }
+
+        return "version:" + version + ", asn:" + asn + ", holdTime:" + holdTime + ", params[" + buf.toString() + "]";
+    }
 }