diff options
Diffstat (limited to 'proto/bfd/packets.c')
| -rw-r--r-- | proto/bfd/packets.c | 208 |
1 files changed, 208 insertions, 0 deletions
diff --git a/proto/bfd/packets.c b/proto/bfd/packets.c new file mode 100644 index 00000000..e48a5aaa --- /dev/null +++ b/proto/bfd/packets.c @@ -0,0 +1,208 @@ + + +#define BFD_FLAG_POLL (1 << 5) +#define BFD_FLAG_FINAL (1 << 4) +#define BFD_FLAG_CPI (1 << 3) +#define BFD_FLAG_AP (1 << 2) +#define BFD_FLAG_DEMAND (1 << 1) +#define BFD_FLAG_MULTIPOINT (1 << 0) + + +struct bfd_ctl_packet +{ + u8 vdiag; /* version and diagnostic */ + u8 flags; /* state and flags */ + u8 detect_mult; + u8 length; + u32 snd_id; /* sender ID, aka 'my discriminator' */ + u32 rcv_id; /* receiver ID, aka 'your discriminator' */ + u32 des_min_tx_int; + u32 req_min_rx_int; + u32 req_min_echo_rx_int; +}; + + +static inline void bfd_pack_vdiag(u8 version, u8 diag) +{ return (version << 5) | diag; } + +static inline void bfd_pack_flags(u8 state, u8 flags) +{ return (state << 6) | diag; } + +static inline u8 bfd_pkt_get_version(struct bfd_ctl_packet *pkt) +{ return pkt->vdiag >> 5; } + +static inline u8 bfd_pkt_get_diag(struct bfd_ctl_packet *pkt) +{ return pkt->vdiag && 0x1f; } + + +static inline u8 bfd_pkt_get_state(struct bfd_ctl_packet *pkt) +{ return pkt->flags >> 6; } + +static inline void bfd_pkt_set_state(struct bfd_ctl_packet *pkt, u8 val) +{ pkt->flags = val << 6; } + + +void +bfd_send_ctl(struct bfd_proto *p, struct bfd_session *s, int final) +{ + sock *sk = p->skX; + struct bfd_ctl_packet *pkt = (struct ospf_packet *) sk->tbuf; + + pkt->vdiag = bfd_pack_vdiag(1, s->loc_diag); + pkt->flags = bfd_pack_flags(s->loc_state, 0); + pkt->detect_mult = s->detect_mult; + pkt->length = 24; + pkt->snd_id = htonl(s->loc_id); + pkt->rcv_id = htonl(s->rem_id); + pkt->des_min_tx_int = htonl(s->des_min_tx_int); + pkt->req_min_rx_int = htonl(s->req_min_rx_int); + pkt->req_min_echo_rx_int = 0; + + if (final) + pkt->flags |= BFD_FLAG_FINAL; + else if (s->poll_active) + pkt->flags |= BFD_FLAG_POLL; + + // XXX + sk_send_to(sk, len, dst, 0); +} + +int +bfd_ctl_rx_hook(sock *sk, int len) +{ + struct bfd_proto *p = sk->data; + struct bfd_ctl_packet *pkt =sk->rbuf; + + if (len < BFD_BASE_LEN) + DROP("too short", len); + + u8 version = bfd_pkt_get_version(pkt); + if (version != 1) + DROP("version mismatch", version); + + if ((pkt->length < BFD_BASE_LEN) || (pkt->length > len)) + DROP("length mismatch", pkt->length); + + if (pkt->detect_mult == 0) + DROP("invalid detect mult", 0); + + if (pkt->flags & BFD_FLAG_MULTIPOINT) + DROP("invalid flags", pkt->flags); + + if (pkt->snd_id == 0) + DROP("invalid my discriminator", 0); + + struct bfd_session *s; + u32 id = ntohl(pkt->rcv_id); + + if (id) + { + s = bfd_find_session_by_id(p, id); + + if (!s) + DROP("unknown session", id); + } + else + { + u8 ps = bfd_pkt_get_state(pkt); + if (ps > BFD_STATE_DOWN) + DROP("invalid init state", ps); + + s = bfd_find_session_by_ip(p, sk->faddr); + + /* FIXME: better session matching and message */ + if (!s || !s->opened) + return; + } + + /* FIXME: better authentication handling and message */ + if (pkt->flags & BFD_FLAG_AP) + DROP("authentication not supported", 0); + + + u32 old_rx_int = s->des_min_tx_int; + u32 old_tx_int = s->rem_min_rx_int; + + s->rem_id = ntohl(pkt->snd_id); + s->rem_state = bfd_pkt_get_state(pkt); + s->rem_demand_mode = pkt->flags & BFD_FLAG_DEMAND; + s->rem_min_tx_int = ntohl(pkt->des_min_tx_int); + s->rem_min_rx_int = ntohl(pkt->req_min_rx_int); + s->rem_detect_mult = pkt->detect_mult; + + bfd_session_process_ctl(s, pkt->flags, xxx); + return 1; + + drop: + // log(L_WARN "%s: Bad packet from %I - %s (%u)", p->p.name, sk->faddr, err_dsc, err_val); + return 1; +} + +sock * +bfd_open_rx_sk(struct bfd_proto *p, int multihop) +{ + sock *sk = sk_new(p->p.pool); + sk->type = SK_UDP; + sk->sport = !multihop ? BFD_CONTROL_PORT : BFD_MULTI_CTL_PORT; + sk->data = p; + + sk->rbsize = 64; // XXX + sk->rx_hook = bfd_rx_hook; + sk->err_hook = bfd_err_hook; + + sk->flags = SKF_LADDR_RX | (!multihop ? SKF_TTL_RX : 0); + + if (sk_open(sk) < 0) + goto err; +} + +static inline sock * +bfd_open_tx_sk(struct bfd_proto *p, ip_addr local, struct iface *ifa) +{ + sock *sk = sk_new(p->p.pool); + sk->type = SK_UDP; + sk->saddr = local; + sk->data = p; + + sk->tbsize = 64; // XXX + sk->err_hook = bfd_err_hook; + + sk->iface = new; + + sk->tos = PATT->tx_tos; + sk->priority = PATT->tx_priority; + sk->ttl = PATT->ttl_security ? 255 : 1; + + if (sk_open(sk) < 0) + goto err; + +} + +struct bfd_socket * +bfd_get_socket(struct bfd_proto *p, ip_addr local, struct iface *ifa) +{ + struct bfd_socket *sk; + + WALK_LIST(sk, p->sockets) + if (ipa_equal(sk->sk->saddr, local) && (sk->sk->iface == ifa)) + return sk->uc++, sk; + + sk = mb_allocz(p->p.pool, sizeof(struct bfd_socket)); + sk->sk = bfd_open_tx_sk(p, local, ifa); + sk->uc = 1; + add_tail(&p->sockets, &sk->n); + + return sk; +} + +void +bfd_free_socket(struct bfd_socket *sk) +{ + if (!sk || --sk->uc) + return; + + rem_node(&sk->n); + sk_stop(sk->sk); + rfree(sk->sk); + mb_free(sk); +} |