UDP Hole Punching Java

client.java

    public static void main(String[] args){
        try{
            DatagramSocket s = new DatagramSocket();

            byte[] b = new byte[10];
            DatagramPacket p = new DatagramPacket(b, b.length, InetAddress.getLocalHost(), 8000);
            s.send(p);

            p = new DatagramPacket(new byte[65535], 65535);
            s.receive(p);

            Location l = getInet(p.getData());

            int lPort = s.getLocalPort();
            s.close();

            System.out.println(l.getA().getHostAddress()+":"+l.getP()+"  "+lPort);

            s = new DatagramSocket(lPort);

            s.setSoTimeout(1000);

            for(int i = 0; i < 4; i++){
                b = ("SEND: "+i).getBytes();
                p = new DatagramPacket(b, b.length, l.getA(), l.getP());
                s.send(p);

                try{
                    p.setData(new byte[1024]);
                    s.receive(p);
                    System.out.println("REC: "+new String(p.getData()));

                }catch(Exception e){
                    System.out.println("SERVER TIMED OUT");
                }
            }

            s.close();
        }catch(Exception e){
            e.printStackTrace();
        }
    }

    public static Location getInet(byte[] b)throws UnknownHostException {
        if(b[0] == 0x04){
            byte[] n = {
                    b[1],
                    b[2],
                    b[3],
                    b[4]
            };
            int p = (((b[5] & 0xff) << 24) | ((b[6] & 0xff) << 16) | ((b[7] & 0xff) << 8) | (b[8] & 0xff));
            return new Location(InetAddress.getByAddress(n), p);

        }else if(b[0] == 0x06){
            byte[] n = {
                    b[1],
                    b[2],
                    b[3],
                    b[4],

                    b[5],
                    b[6],
                    b[7],
                    b[8],

                    b[9],
                    b[10],
                    b[11],
                    b[12],

                    b[13],
                    b[15],
                    b[15],
                    b[16]
            };
            int p = (((b[17] & 0xff) << 24) | ((b[18] & 0xff) << 16) | ((b[19] & 0xff) << 8) | (b[20] & 0xff));
            return new Location(InetAddress.getByAddress(n), p);
        }

        return null;
    }

    static class Location {

        private InetAddress a;
        private int p;

        public Location(InetAddress a, int p){
            this.a = a;
            this.p = p;
        }

        public InetAddress getA(){
            return a;
        }

        public int getP(){
            return p;
        }
    }

server.java

    public static void main(String[] args){
        try{
            DatagramSocket s = new DatagramSocket(8000);

            //CLIENT A
            DatagramPacket p = new DatagramPacket(new byte[65535], 65535);
            s.receive(p);

            System.out.println("A: "+p.getAddress().getHostAddress()+":"+p.getPort());

            //CLIENT B
            DatagramPacket p1 = new DatagramPacket(new byte[65535], 65535);
            s.receive(p1);

            System.out.println("B: "+p1.getAddress().getHostAddress()+":"+p1.getPort());

            byte[] b = getBuf(p.getAddress(), p.getPort());
            DatagramPacket t = new DatagramPacket(b, b.length, p1.getAddress(), p1.getPort());
            s.send(t);

            b = getBuf(p1.getAddress(), p1.getPort());
            t = new DatagramPacket(b, b.length, p.getAddress(), p.getPort());
            s.send(t);

            s.close();
        }catch(Exception e){
            e.printStackTrace();
        }
    }

    public static byte[] getBuf(InetAddress a, int p){
        byte[] buf = new byte[1024];
        if(a instanceof Inet4Address){
            buf[0] = 0x04;

            buf[1] = a.getAddress()[0];
            buf[2] = a.getAddress()[1];
            buf[3] = a.getAddress()[2];
            buf[4] = a.getAddress()[3];


            buf[5] = (byte) (0xff & (p >> 24));
            buf[6] = (byte) (0xff & (p >> 16));
            buf[7] = (byte) (0xff & (p >> 8));
            buf[8] = (byte) (0xff & p);

        }else{
            buf[0] = 0x06;

            buf[1] = a.getAddress()[0];
            buf[2] = a.getAddress()[1];
            buf[3] = a.getAddress()[2];
            buf[4] = a.getAddress()[3];

            buf[5] = a.getAddress()[4];
            buf[6] = a.getAddress()[5];
            buf[7] = a.getAddress()[6];
            buf[8] = a.getAddress()[7];

            buf[9] = a.getAddress()[8];
            buf[10] = a.getAddress()[9];
            buf[11] = a.getAddress()[10];
            buf[12] = a.getAddress()[11];

            buf[13] = a.getAddress()[12];
            buf[14] = a.getAddress()[13];
            buf[15] = a.getAddress()[14];
            buf[16] = a.getAddress()[15];

            buf[17] = (byte) (0xff & (p >> 24));
            buf[18] = (byte) (0xff & (p >> 16));
            buf[19] = (byte) (0xff & (p >> 8));
            buf[20] = (byte) (0xff & p);
        }

        return buf;
    }