From 5455abb3e575a1f6a0458097fb84857871d2f544 Mon Sep 17 00:00:00 2001 From: Felix Fietkau Date: Wed, 11 Aug 2010 01:44:32 +0200 Subject: [PATCH 1/1] Initial import --- Makefile | 17 + list.h | 601 ++++++++++++++++++++++++++++++++++ main.c | 957 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ uloop.c | 268 ++++++++++++++++ uloop.h | 66 ++++ 5 files changed, 1909 insertions(+) create mode 100644 Makefile create mode 100644 list.h create mode 100644 main.c create mode 100644 uloop.c create mode 100644 uloop.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..74ff810 --- /dev/null +++ b/Makefile @@ -0,0 +1,17 @@ +CFLAGS = -O2 -Wall -Werror -pedantic --std=gnu99 +CPPFLAGS = -I. + +all: relayd + +relayd: uloop.o main.o + $(CC) -o $@ $^ + +uloop.c: uloop.h +main.c: uloop.h + +%.o: %.c + $(CC) -c $(CPPFLAGS) $(CFLAGS) -o $@ $^ + + +clean: + rm -f relayd *.o diff --git a/list.h b/list.h new file mode 100644 index 0000000..2959a06 --- /dev/null +++ b/list.h @@ -0,0 +1,601 @@ +#ifndef _LINUX_LIST_H +#define _LINUX_LIST_H + +#include +/** + * container_of - cast a member of a structure out to the containing structure + * @ptr: the pointer to the member. + * @type: the type of the container struct this is embedded in. + * @member: the name of the member within the struct. + * + */ +#ifndef container_of +#define container_of(ptr, type, member) ( \ + (type *)( (char *)ptr - offsetof(type,member) )) +#endif + + +/* + * Simple doubly linked list implementation. + * + * Some of the internal functions ("__xxx") are useful when + * manipulating whole lists rather than single entries, as + * sometimes we already know the next/prev entries and we can + * generate better code by using them directly rather than + * using the generic single-entry routines. + */ + +struct list_head { + struct list_head *next, *prev; +}; + +#define LIST_HEAD_INIT(name) { &(name), &(name) } + +#define LIST_HEAD(name) \ + struct list_head name = LIST_HEAD_INIT(name) + +static inline void INIT_LIST_HEAD(struct list_head *list) +{ + list->next = list; + list->prev = list; +} + +/* + * Insert a new entry between two known consecutive entries. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_add(struct list_head *new, + struct list_head *prev, + struct list_head *next) +{ + next->prev = new; + new->next = next; + new->prev = prev; + prev->next = new; +} + +/** + * list_add - add a new entry + * @new: new entry to be added + * @head: list head to add it after + * + * Insert a new entry after the specified head. + * This is good for implementing stacks. + */ +static inline void list_add(struct list_head *new, struct list_head *head) +{ + __list_add(new, head, head->next); +} + + +/** + * list_add_tail - add a new entry + * @new: new entry to be added + * @head: list head to add it before + * + * Insert a new entry before the specified head. + * This is useful for implementing queues. + */ +static inline void list_add_tail(struct list_head *new, struct list_head *head) +{ + __list_add(new, head->prev, head); +} + + +/* + * Delete a list entry by making the prev/next entries + * point to each other. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_del(struct list_head * prev, struct list_head * next) +{ + next->prev = prev; + prev->next = next; +} + +/** + * list_del - deletes entry from list. + * @entry: the element to delete from the list. + * Note: list_empty() on entry does not return true after this, the entry is + * in an undefined state. + */ +static inline void list_del(struct list_head *entry) +{ + __list_del(entry->prev, entry->next); + entry->next = NULL; + entry->prev = NULL; +} + +/** + * list_replace - replace old entry by new one + * @old : the element to be replaced + * @new : the new element to insert + * + * If @old was empty, it will be overwritten. + */ +static inline void list_replace(struct list_head *old, + struct list_head *new) +{ + new->next = old->next; + new->next->prev = new; + new->prev = old->prev; + new->prev->next = new; +} + +static inline void list_replace_init(struct list_head *old, + struct list_head *new) +{ + list_replace(old, new); + INIT_LIST_HEAD(old); +} + +/** + * list_del_init - deletes entry from list and reinitialize it. + * @entry: the element to delete from the list. + */ +static inline void list_del_init(struct list_head *entry) +{ + __list_del(entry->prev, entry->next); + INIT_LIST_HEAD(entry); +} + +/** + * list_move - delete from one list and add as another's head + * @list: the entry to move + * @head: the head that will precede our entry + */ +static inline void list_move(struct list_head *list, struct list_head *head) +{ + __list_del(list->prev, list->next); + list_add(list, head); +} + +/** + * list_move_tail - delete from one list and add as another's tail + * @list: the entry to move + * @head: the head that will follow our entry + */ +static inline void list_move_tail(struct list_head *list, + struct list_head *head) +{ + __list_del(list->prev, list->next); + list_add_tail(list, head); +} + +/** + * list_is_last - tests whether @list is the last entry in list @head + * @list: the entry to test + * @head: the head of the list + */ +static inline int list_is_last(const struct list_head *list, + const struct list_head *head) +{ + return list->next == head; +} + +/** + * list_empty - tests whether a list is empty + * @head: the list to test. + */ +static inline int list_empty(const struct list_head *head) +{ + return head->next == head; +} + +/** + * list_empty_careful - tests whether a list is empty and not being modified + * @head: the list to test + * + * Description: + * tests whether a list is empty _and_ checks that no other CPU might be + * in the process of modifying either member (next or prev) + * + * NOTE: using list_empty_careful() without synchronization + * can only be safe if the only activity that can happen + * to the list entry is list_del_init(). Eg. it cannot be used + * if another CPU could re-list_add() it. + */ +static inline int list_empty_careful(const struct list_head *head) +{ + struct list_head *next = head->next; + return (next == head) && (next == head->prev); +} + +static inline void __list_splice(struct list_head *list, + struct list_head *head) +{ + struct list_head *first = list->next; + struct list_head *last = list->prev; + struct list_head *at = head->next; + + first->prev = head; + head->next = first; + + last->next = at; + at->prev = last; +} + +/** + * list_splice - join two lists + * @list: the new list to add. + * @head: the place to add it in the first list. + */ +static inline void list_splice(struct list_head *list, struct list_head *head) +{ + if (!list_empty(list)) + __list_splice(list, head); +} + +/** + * list_splice_init - join two lists and reinitialise the emptied list. + * @list: the new list to add. + * @head: the place to add it in the first list. + * + * The list at @list is reinitialised + */ +static inline void list_splice_init(struct list_head *list, + struct list_head *head) +{ + if (!list_empty(list)) { + __list_splice(list, head); + INIT_LIST_HEAD(list); + } +} + +/** + * list_entry - get the struct for this entry + * @ptr: the &struct list_head pointer. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_struct within the struct. + */ +#define list_entry(ptr, type, member) \ + container_of(ptr, type, member) + +/** + * list_first_entry - get the first element from a list + * @ptr: the list head to take the element from. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_struct within the struct. + * + * Note, that list is expected to be not empty. + */ +#define list_first_entry(ptr, type, member) \ + list_entry((ptr)->next, type, member) + +/** + * list_for_each - iterate over a list + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + */ +#define list_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); \ + pos = pos->next) + +/** + * __list_for_each - iterate over a list + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + * + * This variant differs from list_for_each() in that it's the + * simplest possible list iteration code, no prefetching is done. + * Use this for code that knows the list to be very short (empty + * or 1 entry) most of the time. + */ +#define __list_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); pos = pos->next) + +/** + * list_for_each_prev - iterate over a list backwards + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + */ +#define list_for_each_prev(pos, head) \ + for (pos = (head)->prev; pos != (head); \ + pos = pos->prev) + +/** + * list_for_each_safe - iterate over a list safe against removal of list entry + * @pos: the &struct list_head to use as a loop cursor. + * @n: another &struct list_head to use as temporary storage + * @head: the head for your list. + */ +#define list_for_each_safe(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, n = pos->next) + +/** + * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry + * @pos: the &struct list_head to use as a loop cursor. + * @n: another &struct list_head to use as temporary storage + * @head: the head for your list. + */ +#define list_for_each_prev_safe(pos, n, head) \ + for (pos = (head)->prev, n = pos->prev; \ + pos != (head); \ + pos = n, n = pos->prev) + +/** + * list_for_each_entry - iterate over list of given type + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry(pos, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_reverse - iterate backwards over list of given type. + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry_reverse(pos, head, member) \ + for (pos = list_entry((head)->prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.prev, typeof(*pos), member)) + +/** + * list_prepare_entry - prepare a pos entry for use in list_for_each_entry_continue() + * @pos: the type * to use as a start point + * @head: the head of the list + * @member: the name of the list_struct within the struct. + * + * Prepares a pos entry for use as a start point in list_for_each_entry_continue(). + */ +#define list_prepare_entry(pos, head, member) \ + ((pos) ? : list_entry(head, typeof(*pos), member)) + +/** + * list_for_each_entry_continue - continue iteration over list of given type + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Continue to iterate over list of given type, continuing after + * the current position. + */ +#define list_for_each_entry_continue(pos, head, member) \ + for (pos = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_continue_reverse - iterate backwards from the given point + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Start to iterate over list of given type backwards, continuing after + * the current position. + */ +#define list_for_each_entry_continue_reverse(pos, head, member) \ + for (pos = list_entry(pos->member.prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.prev, typeof(*pos), member)) + +/** + * list_for_each_entry_from - iterate over list of given type from the current point + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type, continuing from current position. + */ +#define list_for_each_entry_from(pos, head, member) \ + for (; &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry_safe(pos, n, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member), \ + n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_continue + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type, continuing after current point, + * safe against removal of list entry. + */ +#define list_for_each_entry_safe_continue(pos, n, head, member) \ + for (pos = list_entry(pos->member.next, typeof(*pos), member), \ + n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_from + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type from current point, safe against + * removal of list entry. + */ +#define list_for_each_entry_safe_from(pos, n, head, member) \ + for (n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_reverse + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate backwards over list of given type, safe against removal + * of list entry. + */ +#define list_for_each_entry_safe_reverse(pos, n, head, member) \ + for (pos = list_entry((head)->prev, typeof(*pos), member), \ + n = list_entry(pos->member.prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.prev, typeof(*n), member)) + +/* + * Double linked lists with a single pointer list head. + * Mostly useful for hash tables where the two pointer list head is + * too wasteful. + * You lose the ability to access the tail in O(1). + */ + +struct hlist_head { + struct hlist_node *first; +}; + +struct hlist_node { + struct hlist_node *next, **pprev; +}; + +#define HLIST_HEAD_INIT { .first = NULL } +#define HLIST_HEAD(name) struct hlist_head name = { .first = NULL } +#define INIT_HLIST_HEAD(ptr) ((ptr)->first = NULL) +static inline void INIT_HLIST_NODE(struct hlist_node *h) +{ + h->next = NULL; + h->pprev = NULL; +} + +static inline int hlist_unhashed(const struct hlist_node *h) +{ + return !h->pprev; +} + +static inline int hlist_empty(const struct hlist_head *h) +{ + return !h->first; +} + +static inline void __hlist_del(struct hlist_node *n) +{ + struct hlist_node *next = n->next; + struct hlist_node **pprev = n->pprev; + *pprev = next; + if (next) + next->pprev = pprev; +} + +static inline void hlist_del(struct hlist_node *n) +{ + __hlist_del(n); + n->next = NULL; + n->pprev = NULL; +} + +static inline void hlist_del_init(struct hlist_node *n) +{ + if (!hlist_unhashed(n)) { + __hlist_del(n); + INIT_HLIST_NODE(n); + } +} + + +static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h) +{ + struct hlist_node *first = h->first; + n->next = first; + if (first) + first->pprev = &n->next; + h->first = n; + n->pprev = &h->first; +} + + +/* next must be != NULL */ +static inline void hlist_add_before(struct hlist_node *n, + struct hlist_node *next) +{ + n->pprev = next->pprev; + n->next = next; + next->pprev = &n->next; + *(n->pprev) = n; +} + +static inline void hlist_add_after(struct hlist_node *n, + struct hlist_node *next) +{ + next->next = n->next; + n->next = next; + next->pprev = &n->next; + + if(next->next) + next->next->pprev = &next->next; +} + +#define hlist_entry(ptr, type, member) container_of(ptr,type,member) + +#define hlist_for_each(pos, head) \ + for (pos = (head)->first; pos; pos = pos->next) + +#define hlist_for_each_safe(pos, n, head) \ + for (pos = (head)->first; pos; pos = n) + +/** + * hlist_for_each_entry - iterate over list of given type + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry(tpos, pos, head, member) \ + for (pos = (head)->first; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_continue - iterate over a hlist continuing after current point + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_continue(tpos, pos, member) \ + for (pos = (pos)->next; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_from - iterate over a hlist continuing from current point + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_from(tpos, pos, member) \ + for (; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_safe - iterate over list of given type safe against removal of list entry + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @n: another &struct hlist_node to use as temporary storage + * @head: the head for your list. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_safe(tpos, pos, n, head, member) \ + for (pos = (head)->first; \ + pos && ({ n = pos->next; 1; }) && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = n) + +#endif diff --git a/main.c b/main.c new file mode 100644 index 0000000..1593583 --- /dev/null +++ b/main.c @@ -0,0 +1,957 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uloop.h" +#include "list.h" + +#define DEBUG +#ifdef DEBUG +#define DPRINTF(level, ...) if (debug >= level) fprintf(stderr, __VA_ARGS__); +#else +#define DPRINTF(...) do {} while(0) +#endif + +#ifndef __packed +#define __packed __attribute__((packed)) +#endif + +#define __uc(c) ((unsigned char *)(c)) + +#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x" +#define MAC_BUF(_c) __uc(_c)[0], __uc(_c)[1], __uc(_c)[2], __uc(_c)[3], __uc(_c)[4], __uc(_c)[5] + +#define IP_FMT "%d.%d.%d.%d" +#define IP_BUF(_c) __uc(_c)[0], __uc(_c)[1], __uc(_c)[2], __uc(_c)[3] + +#define DUMMY_IP ((uint8_t *) "\x01\x01\x01\x01") + +#define DHCP_FLAG_BROADCAST (1 << 15) + +struct relayd_interface { + struct list_head list; + struct uloop_fd fd; + struct uloop_fd bcast_fd; + struct sockaddr_ll sll; + struct sockaddr_ll bcast_sll; + char ifname[IFNAMSIZ]; + struct list_head hosts; + uint8_t src_ip[4]; + bool managed; +}; + +struct relayd_host { + struct list_head list; + struct relayd_interface *rif; + uint8_t lladdr[ETH_ALEN]; + uint8_t ipaddr[4]; + struct uloop_timeout timeout; + int cleanup_pending; +}; + +struct arp_packet { + struct ether_header eth; + struct ether_arp arp; +} __packed; + +struct ip_packet { + struct ether_header eth; + struct iphdr iph; +} __packed; + +struct dhcp_header { + uint8_t op, htype, hlen, hops; + uint32_t xit; + uint16_t secs, flags; + struct in_addr ciaddr, yiaddr, siaddr, giaddr; + unsigned char chaddr[16]; + unsigned char sname[64]; + unsigned char file[128]; +} __packed; + +struct rtnl_req { + struct nlmsghdr nl; + struct rtmsg rt; +}; + +static int debug; +static LIST_HEAD(interfaces); +static int host_timeout; +static int inet_sock; +static int forward_bcast; +static int forward_dhcp; +static struct uloop_fd rtnl_sock; +static unsigned int rtnl_seq, rtnl_dump_seq; + +static struct relayd_host *find_host_by_ipaddr(struct relayd_interface *rif, const uint8_t *ipaddr) +{ + struct relayd_host *host; + + if (!rif) { + list_for_each_entry(rif, &interfaces, list) { + host = find_host_by_ipaddr(rif, ipaddr); + if (!host) + continue; + + return host; + } + return NULL; + } + + list_for_each_entry(host, &rif->hosts, list) { + if (memcmp(ipaddr, host->ipaddr, sizeof(host->ipaddr)) != 0) + continue; + + return host; + } + return NULL; +} + +static void add_arp(struct relayd_host *host) +{ + struct sockaddr_in *sin; + struct arpreq arp; + + strncpy(arp.arp_dev, host->rif->ifname, sizeof(arp.arp_dev)); + arp.arp_flags = ATF_COM; + + arp.arp_ha.sa_family = ARPHRD_ETHER; + memcpy(arp.arp_ha.sa_data, host->lladdr, ETH_ALEN); + + sin = (struct sockaddr_in *) &arp.arp_pa; + sin->sin_family = AF_INET; + memcpy(&sin->sin_addr, host->ipaddr, sizeof(host->ipaddr)); + + ioctl(inet_sock, SIOCSARP, &arp); +} + +static void rtnl_route_set(struct relayd_host *host, bool add) +{ + static struct { + struct nlmsghdr nl; + struct rtmsg rt; + struct { + struct rtattr rta; + uint8_t ipaddr[4]; + } __packed dst; + struct { + struct rtattr rta; + int ifindex; + } __packed dev; + } __packed req; + + memset(&req, 0, sizeof(req)); + + req.nl.nlmsg_len = sizeof(req); + req.rt.rtm_family = AF_INET; + req.rt.rtm_dst_len = 32; + + req.dst.rta.rta_type = RTA_DST; + req.dst.rta.rta_len = sizeof(req.dst); + memcpy(req.dst.ipaddr, host->ipaddr, sizeof(req.dst.ipaddr)); + + req.dev.rta.rta_type = RTA_OIF; + req.dev.rta.rta_len = sizeof(req.dev); + req.dev.ifindex = host->rif->sll.sll_ifindex; + + req.nl.nlmsg_flags = NLM_F_REQUEST; + req.rt.rtm_table = RT_TABLE_MAIN; + if (add) { + req.nl.nlmsg_type = RTM_NEWROUTE; + req.nl.nlmsg_flags |= NLM_F_CREATE | NLM_F_REPLACE; + + req.rt.rtm_protocol = RTPROT_BOOT; + req.rt.rtm_scope = RT_SCOPE_LINK; + req.rt.rtm_type = RTN_UNICAST; + } else { + req.nl.nlmsg_type = RTM_DELROUTE; + req.rt.rtm_scope = RT_SCOPE_NOWHERE; + } + + send(rtnl_sock.fd, &req, sizeof(req), 0); +} + +static void add_route(struct relayd_host *host) +{ + rtnl_route_set(host, true); +} + +static void del_route(struct relayd_host *host) +{ + rtnl_route_set(host, false); +} + +static void del_host(struct relayd_host *host) +{ + DPRINTF(1, "%s: deleting host "IP_FMT" ("MAC_FMT")\n", host->rif->ifname, + IP_BUF(host->ipaddr), MAC_BUF(host->lladdr)); + + if (host->rif->managed) + del_route(host); + list_del(&host->list); + free(host); +} + +static void fill_arp_request(struct arp_packet *pkt, struct relayd_interface *rif, + uint8_t spa[4], uint8_t tpa[4]) +{ + memset(pkt, 0, sizeof(*pkt)); + + pkt->eth.ether_type = htons(ETHERTYPE_ARP); + memcpy(pkt->eth.ether_shost, rif->sll.sll_addr, ETH_ALEN); + + memcpy(pkt->arp.arp_sha, rif->sll.sll_addr, ETH_ALEN); + memcpy(pkt->arp.arp_spa, spa, 4); + memcpy(pkt->arp.arp_tpa, tpa, 4); + + pkt->arp.arp_hrd = htons(ARPHRD_ETHER); + pkt->arp.arp_pro = htons(ETH_P_IP); + pkt->arp.arp_hln = ETH_ALEN; + pkt->arp.arp_pln = 4; +} + +static void send_arp_request(struct relayd_host *host) +{ + struct relayd_interface *rif = host->rif; + struct arp_packet pkt; + + fill_arp_request(&pkt, host->rif, host->rif->src_ip, host->ipaddr); + + pkt.arp.arp_op = htons(ARPOP_REQUEST); + memcpy(pkt.arp.arp_spa, rif->src_ip, ETH_ALEN); + memset(pkt.arp.arp_tha, 0, ETH_ALEN); + memset(pkt.eth.ether_dhost, 0xff, ETH_ALEN); + + DPRINTF(2, "%s: sending ARP who-has "IP_FMT", tell "IP_FMT" ("MAC_FMT")\n", + rif->ifname, IP_BUF(pkt.arp.arp_tpa), + IP_BUF(pkt.arp.arp_spa), MAC_BUF(pkt.eth.ether_shost)); + + sendto(rif->fd.fd, &pkt, sizeof(pkt), 0, + (struct sockaddr *) &rif->sll, sizeof(rif->sll)); +} + +static void send_arp_reply(struct relayd_interface *rif, uint8_t spa[4], + uint8_t tha[ETH_ALEN], uint8_t tpa[4]) +{ + struct arp_packet pkt; + + fill_arp_request(&pkt, rif, spa, tpa); + + pkt.arp.arp_op = htons(ARPOP_REPLY); + memcpy(pkt.eth.ether_dhost, tha, ETH_ALEN); + memcpy(pkt.arp.arp_tha, tha, ETH_ALEN); + + DPRINTF(2, "%s: sending ARP reply to "IP_FMT", "IP_FMT" is at ("MAC_FMT")\n", + rif->ifname, IP_BUF(pkt.arp.arp_tpa), + IP_BUF(pkt.arp.arp_spa), MAC_BUF(pkt.eth.ether_shost)); + + sendto(rif->fd.fd, &pkt, sizeof(pkt), 0, + (struct sockaddr *) &rif->sll, sizeof(rif->sll)); +} + +static void host_entry_timeout(struct uloop_timeout *timeout) +{ + struct relayd_host *host = container_of(timeout, struct relayd_host, timeout); + + /* + * When a host is behind a managed interface, we must not expire its host + * entry prematurely, as this will cause routes to the node to expire, + * leading to loss of connectivity from the other side. + * When the timeout is reached, try pinging the host a few times before + * giving up on it. + */ + if (host->rif->managed && host->cleanup_pending < 2) { + send_arp_request(host); + host->cleanup_pending++; + uloop_timeout_set(&host->timeout, 1000); + return; + } + del_host(host); +} + +static struct relayd_host *add_host(struct relayd_interface *rif, const uint8_t *lladdr, const uint8_t *ipaddr) +{ + struct relayd_host *host; + + DPRINTF(1, "%s: adding host "IP_FMT" ("MAC_FMT")\n", rif->ifname, + IP_BUF(ipaddr), MAC_BUF(lladdr)); + + host = calloc(1, sizeof(*host)); + host->rif = rif; + memcpy(host->ipaddr, ipaddr, sizeof(host->ipaddr)); + memcpy(host->lladdr, lladdr, sizeof(host->lladdr)); + list_add(&host->list, &rif->hosts); + host->timeout.cb = host_entry_timeout; + uloop_timeout_set(&host->timeout, host_timeout * 1000); + + add_arp(host); + if (rif->managed) + add_route(host); + + return host; +} + +static struct relayd_host *refresh_host(struct relayd_interface *rif, const uint8_t *lladdr, const uint8_t *ipaddr) +{ + struct relayd_host *host; + + host = find_host_by_ipaddr(rif, ipaddr); + if (!host) { + host = find_host_by_ipaddr(NULL, ipaddr); + + /* + * When we suddenly see the host appearing on a different interface, + * reduce the timeout to make the old entry expire faster, in case the + * host has moved. + * If the old entry is behind a managed interface, it will be pinged + * before we expire it + */ + if (host && !host->cleanup_pending) + uloop_timeout_set(&host->timeout, 1); + + host = add_host(rif, lladdr, ipaddr); + } else { + host->cleanup_pending = false; + uloop_timeout_set(&host->timeout, host_timeout * 1000); + } + + return host; +} + +static void relay_arp_request(struct relayd_interface *from_rif, struct arp_packet *pkt) +{ + struct relayd_interface *rif; + struct arp_packet reqpkt; + + memcpy(&reqpkt, pkt, sizeof(reqpkt)); + list_for_each_entry(rif, &interfaces, list) { + if (rif == from_rif) + continue; + + memcpy(reqpkt.eth.ether_shost, rif->sll.sll_addr, ETH_ALEN); + memcpy(reqpkt.arp.arp_sha, rif->sll.sll_addr, ETH_ALEN); + + DPRINTF(2, "%s: sending ARP who-has "IP_FMT", tell "IP_FMT" ("MAC_FMT")\n", + rif->ifname, IP_BUF(reqpkt.arp.arp_tpa), + IP_BUF(reqpkt.arp.arp_spa), MAC_BUF(reqpkt.eth.ether_shost)); + + sendto(rif->fd.fd, &reqpkt, sizeof(reqpkt), 0, + (struct sockaddr *) &rif->sll, sizeof(rif->sll)); + } +} + +static void recv_arp_request(struct relayd_interface *rif, struct arp_packet *pkt) +{ + struct relayd_host *host; + + DPRINTF(2, "%s: ARP who-has "IP_FMT", tell "IP_FMT" ("MAC_FMT")\n", + rif->ifname, + IP_BUF(pkt->arp.arp_tpa), + IP_BUF(pkt->arp.arp_spa), + MAC_BUF(pkt->eth.ether_shost)); + + if (!memcmp(pkt->arp.arp_spa, "\x00\x00\x00\x00", 4)) + return; + + refresh_host(rif, pkt->eth.ether_shost, pkt->arp.arp_spa); + + host = find_host_by_ipaddr(NULL, pkt->arp.arp_tpa); + + /* + * If a host is being pinged because of a timeout, do not use the cached + * entry here. That way we can avoid giving out stale data in case the node + * has moved. We shouldn't relay requests here either, as we might miss our + * chance to create a host route. + */ + if (host && host->cleanup_pending) + return; + + relay_arp_request(rif, pkt); +} + + +static void recv_arp_reply(struct relayd_interface *rif, struct arp_packet *pkt) +{ + struct relayd_host *host; + + DPRINTF(2, "%s: received ARP reply for "IP_FMT" from "MAC_FMT", deliver to "IP_FMT"\n", + rif->ifname, + IP_BUF(pkt->arp.arp_spa), + MAC_BUF(pkt->eth.ether_shost), + IP_BUF(pkt->arp.arp_tpa)); + + refresh_host(rif, pkt->arp.arp_sha, pkt->arp.arp_spa); + + if (!memcmp(pkt->arp.arp_tpa, rif->src_ip, 4)) + return; + + host = find_host_by_ipaddr(NULL, pkt->arp.arp_tpa); + if (!host) + return; + + send_arp_reply(host->rif, pkt->arp.arp_spa, host->lladdr, host->ipaddr); +} + +static void recv_packet(struct uloop_fd *fd, unsigned int events) +{ + struct relayd_interface *rif = container_of(fd, struct relayd_interface, fd); + struct arp_packet *pkt; + static char pktbuf[4096]; + int pktlen; + + do { + if (rif->fd.error) + uloop_end(); + + pktlen = recv(rif->fd.fd, pktbuf, sizeof(pktbuf), 0); + if (pktlen < 0) { + if (errno == EINTR) + continue; + + break; + } + + if (!pktlen) + break; + + pkt = (void *)pktbuf; + if (pkt->arp.arp_op == htons(ARPOP_REPLY)) + recv_arp_reply(rif, pkt); + else if (pkt->arp.arp_op == htons(ARPOP_REQUEST)) + recv_arp_request(rif, pkt); + else + DPRINTF(1, "received unknown packet type: %04x\n", ntohs(pkt->arp.arp_op)); + + } while (1); +} + +static void forward_bcast_packet(struct relayd_interface *from_rif, void *packet, int len) +{ + struct relayd_interface *rif; + struct ether_header *eth = packet; + + list_for_each_entry(rif, &interfaces, list) { + if (rif == from_rif) + continue; + + DPRINTF(3, "%s: forwarding broadcast packet to %s\n", from_rif->ifname, rif->ifname); + memcpy(eth->ether_shost, rif->sll.sll_addr, ETH_ALEN); + send(rif->bcast_fd.fd, packet, len, 0); + } +} + +static uint16_t +chksum(uint16_t sum, const uint8_t *data, uint16_t len) +{ + const uint8_t *last; + uint16_t t; + + last = data + len - 1; + + while(data < last) { + t = (data[0] << 8) + data[1]; + sum += t; + if(sum < t) + sum++; + data += 2; + } + + if(data == last) { + t = (data[0] << 8) + 0; + sum += t; + if(sum < t) + sum++; + } + + return sum; +} + +static bool forward_dhcp_packet(struct relayd_interface *rif, void *data, int len) +{ + struct ip_packet *pkt = data; + struct udphdr *udp; + struct dhcp_header *dhcp; + int udplen; + uint16_t sum; + + if (pkt->eth.ether_type != htons(ETH_P_IP)) + return false; + + if (pkt->iph.version != 4) + return false; + + if (pkt->iph.protocol != IPPROTO_UDP) + return false; + + udp = (void *) ((char *) &pkt->iph + (pkt->iph.ihl << 2)); + dhcp = (void *) (udp + 1); + + udplen = ntohs(udp->len); + if (udplen > len - ((char *) udp - (char *) data)) + return false; + + if (udp->dest != htons(67) && udp->source != htons(67)) + return false; + + if (dhcp->op != 1 && dhcp->op != 2) + return false; + + if (!forward_dhcp) + return true; + + DPRINTF(2, "%s: handling DHCP %s\n", rif->ifname, (dhcp->op == 1 ? "request" : "response")); + + dhcp->flags |= htons(DHCP_FLAG_BROADCAST); + + udp->check = 0; + sum = udplen + IPPROTO_UDP; + sum = chksum(sum, (void *) &pkt->iph.saddr, 8); + sum = chksum(sum, (void *) udp, udplen); + if (sum == 0) + sum = 0xffff; + + udp->check = htons(~sum); + + forward_bcast_packet(rif, data, len); + + return true; +} + +static void recv_bcast_packet(struct uloop_fd *fd, unsigned int events) +{ + struct relayd_interface *rif = container_of(fd, struct relayd_interface, bcast_fd); + static char pktbuf[4096]; + int pktlen; + + do { + if (rif->fd.error) + uloop_end(); + + pktlen = recv(rif->bcast_fd.fd, pktbuf, sizeof(pktbuf), 0); + if (pktlen < 0) { + if (errno == EINTR) + continue; + + break; + } + + if (!pktlen) + break; + + if (!forward_bcast && !forward_dhcp) + continue; + + if (forward_dhcp_packet(rif, pktbuf, pktlen)) + continue; + + if (forward_bcast) + forward_bcast_packet(rif, pktbuf, pktlen); + } while (1); +} + + +static int init_interface(struct relayd_interface *rif) +{ + struct sockaddr_ll *sll = &rif->sll; + struct sockaddr_in *sin; + struct ifreq ifr; + int fd = rif->fd.fd; +#ifdef PACKET_RECV_TYPE + unsigned int pkt_type; +#endif + + fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ARP)); + if (fd < 0) + return -1; + + rif->fd.fd = fd; + + memset(&ifr, 0, sizeof(ifr)); + strcpy(ifr.ifr_name, rif->ifname); + + if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) { + perror("ioctl(SIOCGIFHWADDR)"); + return -1; + } + + memcpy(sll->sll_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN); + sll->sll_family = AF_PACKET; + sll->sll_protocol = htons(ETH_P_ARP); + sll->sll_pkttype = PACKET_BROADCAST; + sll->sll_hatype = ARPHRD_ETHER; + sll->sll_halen = ETH_ALEN; + + if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + perror("ioctl(SIOCGIFINDEX)"); + return -1; + } + + sll->sll_ifindex = ifr.ifr_ifindex; + + if (ioctl(fd, SIOCGIFADDR, &ifr) < 0) { + memcpy(rif->src_ip, DUMMY_IP, sizeof(rif->src_ip)); + } else { + sin = (struct sockaddr_in *) &ifr.ifr_addr; + memcpy(rif->src_ip, &sin->sin_addr.s_addr, sizeof(rif->src_ip)); + } + + if (bind(fd, (struct sockaddr *)sll, sizeof(struct sockaddr_ll)) < 0) { + perror("bind(ETH_P_ARP)"); + return -1; + } + + rif->fd.cb = recv_packet; + uloop_fd_add(&rif->fd, ULOOP_READ | ULOOP_EDGE_TRIGGER); + + if (!forward_bcast && !forward_dhcp) + return 0; + + fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_IP)); + if (fd < 0) + return 0; + + rif->bcast_fd.fd = fd; + rif->bcast_fd.cb = recv_bcast_packet; + + memcpy(&rif->bcast_sll, &rif->sll, sizeof(rif->bcast_sll)); + sll = &rif->bcast_sll; + sll->sll_protocol = htons(ETH_P_IP); + + if (bind(fd, (struct sockaddr *)sll, sizeof(struct sockaddr_ll)) < 0) { + perror("bind(ETH_P_IP)"); + return 0; + } + +#ifdef PACKET_RECV_TYPE + pkt_type = (1 << PACKET_BROADCAST); + setsockopt(fd, SOL_PACKET, PACKET_RECV_TYPE, &pkt_type, sizeof(pkt_type)); +#endif + + uloop_fd_add(&rif->bcast_fd, ULOOP_READ | ULOOP_EDGE_TRIGGER); + return 0; +} + +static int init_interfaces(void) +{ + struct relayd_interface *rif; + int ret; + + list_for_each_entry(rif, &interfaces, list) { + ret = init_interface(rif); + if (ret < 0) + return ret; + } + + return 0; +} + +static void del_interface(struct relayd_interface *rif) +{ + struct relayd_host *host, *htmp; + + list_for_each_entry_safe(host, htmp, &rif->hosts, list) { + del_host(host); + } + free(rif); +} + +static void cleanup_interfaces(void) +{ + struct relayd_interface *rif, *rtmp; + + list_for_each_entry_safe(rif, rtmp, &interfaces, list) { + del_interface(rif); + } +} + +static int alloc_interface(const char *ifname, bool managed) +{ + struct relayd_interface *rif; + + if (strlen(ifname) >= IFNAMSIZ) + return -1; + + rif = calloc(1, sizeof(*rif)); + if (!rif) + return -1; + + INIT_LIST_HEAD(&rif->list); + INIT_LIST_HEAD(&rif->hosts); + strcpy(rif->ifname, ifname); + list_add(&rif->list, &interfaces); + rif->managed = managed; + + return 0; +} + +#ifndef NDA_RTA +#define NDA_RTA(r) \ + ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndmsg)))) +#endif + +static void rtnl_parse_newneigh(struct nlmsghdr *h) +{ + struct relayd_interface *rif = NULL; + struct ndmsg *r = NLMSG_DATA(h); + const uint8_t *lladdr = NULL; + const uint8_t *ipaddr = NULL; + struct rtattr *rta; + int len; + + if (r->ndm_family != AF_INET) + return; + + list_for_each_entry(rif, &interfaces, list) { + if (rif->sll.sll_ifindex == r->ndm_ifindex) + goto found_interface; + } + return; + +found_interface: + len = h->nlmsg_len - NLMSG_LENGTH(sizeof(*r)); + for (rta = NDA_RTA(r); RTA_OK(rta, len); rta = RTA_NEXT(rta, len)) { + switch(rta->rta_type) { + case NDA_LLADDR: + lladdr = RTA_DATA(rta); + break; + case NDA_DST: + ipaddr = RTA_DATA(rta); + break; + default: + break; + } + } + + if (!lladdr || !ipaddr || (r->ndm_state & (NUD_INCOMPLETE|NUD_FAILED))) + return; + + if (!memcmp(lladdr, "\x00\x00\x00\x00\x00\x00", ETH_ALEN)) + return; + + DPRINTF(1, "%s: Found ARP cache entry for host "IP_FMT" ("MAC_FMT")\n", + rif->ifname, IP_BUF(ipaddr), MAC_BUF(lladdr)); + refresh_host(rif, lladdr, ipaddr); +} + +static void rtnl_parse_packet(void *data, int len) +{ + struct nlmsghdr *h; + + for (h = data; NLMSG_OK(h, len); h = NLMSG_NEXT(h, len)) { + if (h->nlmsg_type == NLMSG_DONE || + h->nlmsg_type == NLMSG_ERROR) + return; + + if (h->nlmsg_seq != rtnl_dump_seq) + continue; + + if (h->nlmsg_type == RTM_NEWNEIGH) + rtnl_parse_newneigh(h); + } +} + +static void rtnl_cb(struct uloop_fd *fd, unsigned int events) +{ + struct sockaddr_nl nladdr; + static uint8_t buf[16384]; + struct iovec iov = { + .iov_base = buf, + .iov_len = sizeof(buf), + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + + do { + int len; + + len = recvmsg(rtnl_sock.fd, &msg, 0); + if (len < 0) { + if (errno == EINTR) + continue; + + return; + } + + if (!len) + break; + + if (nladdr.nl_pid != 0) + continue; + + rtnl_parse_packet(buf, len); + } while (1); +} + +static int rtnl_init(void) +{ + struct sockaddr_nl snl_local; + static struct { + struct nlmsghdr nlh; + struct rtgenmsg g; + } req = { + .nlh = { + .nlmsg_len = sizeof(req), + .nlmsg_type = RTM_GETNEIGH, + .nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST, + .nlmsg_pid = 0, + }, + .g.rtgen_family = AF_INET, + }; + + rtnl_sock.fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + if (rtnl_sock.fd < 0) { + perror("socket(AF_NETLINK)"); + return -1; + } + + snl_local.nl_family = AF_NETLINK; + + if (bind(rtnl_sock.fd, (struct sockaddr *) &snl_local, sizeof(struct sockaddr_nl)) < 0) { + perror("bind"); + close(rtnl_sock.fd); + return -1; + } + + rtnl_sock.cb = rtnl_cb; + uloop_fd_add(&rtnl_sock, ULOOP_READ | ULOOP_EDGE_TRIGGER); + + rtnl_seq = time(NULL); + rtnl_dump_seq = rtnl_seq; + req.nlh.nlmsg_seq = rtnl_seq; + send(rtnl_sock.fd, &req, sizeof(req), 0); + + return 0; +} + +static void die(int signo) +{ + /* + * When we hit SIGTERM, clean up interfaces directly, so that we + * won't leave our routing in an invalid state. + */ + cleanup_interfaces(); + exit(1); +} + +static int usage(const char *progname) +{ + fprintf(stderr, "Usage: %s \n" + "\n" + "Options:\n" + " -d Enable debug messages\n" + " -i Add an interface for relaying\n" + " -I Same as -i, except with ARP cache and host route management\n" + " You need to specify at least two interfaces\n" + " -t Host entry expiry timeout\n" + " -B Enable broadcast forwarding\n" + " -D Enable DHCP forwarding\n" + "\n", + progname); + return -1; +} + +int main(int argc, char **argv) +{ + bool managed; + int ifnum = 0; + int ch; + + debug = 0; + inet_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (inet_sock < 0) { + perror("socket(AF_INET)"); + return 1; + } + + host_timeout = 60; + forward_bcast = 0; + uloop_init(); + + while ((ch = getopt(argc, argv, "I:i:t:BDd")) != -1) { + switch(ch) { + case 'I': + managed = true; + /* fall through */ + case 'i': + ifnum++; + if (alloc_interface(optarg, managed) < 0) + return 1; + + managed = false; + break; + case 't': + host_timeout = atoi(optarg); + if (host_timeout <= 0) + return usage(argv[0]); + break; + case 'd': + debug++; + break; + case 'B': + forward_bcast = 1; + break; + case 'D': + forward_dhcp = 1; + break; + case '?': + default: + return usage(argv[0]); + } + } + + if (list_empty(&interfaces)) + return usage(argv[0]); + + if (ifnum < 2) { + fprintf(stderr, "ERROR: Need at least 2 interfaces for relaying\n"); + return -1; + } + + argc -= optind; + argv += optind; + + signal(SIGTERM, die); + signal(SIGHUP, die); + signal(SIGUSR1, die); + signal(SIGUSR2, die); + + if (init_interfaces() < 0) + return 1; + + if (rtnl_init() < 0) + return 1; + + uloop_run(); + uloop_done(); + + cleanup_interfaces(); + uloop_fd_delete(&rtnl_sock); + close(rtnl_sock.fd); + close(inet_sock); + + return 0; +} diff --git a/uloop.c b/uloop.c new file mode 100644 index 0000000..8c85906 --- /dev/null +++ b/uloop.c @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2010 Felix Fietkau + * Copyright (C) 2010 John Crispin + * Copyright (C) 2010 Steven Barth + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA. + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uloop.h" + +/** + * FIXME: uClibc < 0.9.30.3 does not define EPOLLRDHUP for Linux >= 2.6.17 + */ +#ifndef EPOLLRDHUP +#define EPOLLRDHUP 0x2000 +#endif + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) +#endif + +struct uloop_timeout *first_timeout; +static int epoll_fd; +static bool cancel; + +int uloop_fd_add(struct uloop_fd *sock, unsigned int flags) +{ + struct epoll_event ev; + int op = sock->registered ? EPOLL_CTL_MOD : EPOLL_CTL_ADD; + unsigned int fl; + int ret; + + fl = fcntl(sock->fd, F_GETFL, 0); + fl |= O_NONBLOCK; + fcntl(sock->fd, F_SETFL, fl); + + memset(&ev, 0, sizeof(struct epoll_event)); + + if (flags & ULOOP_READ) + ev.events |= EPOLLIN | EPOLLRDHUP; + + if (flags & ULOOP_WRITE) + ev.events |= EPOLLOUT; + + if (flags & ULOOP_EDGE_TRIGGER) + ev.events |= EPOLLET; + + ev.data.fd = sock->fd; + ev.data.ptr = sock; + + ret = epoll_ctl(epoll_fd, op, sock->fd, &ev); + if (ret < 0) + goto out; + + sock->registered = true; + sock->eof = false; + +out: + return ret; +} + +int uloop_fd_delete(struct uloop_fd *sock) +{ + sock->registered = false; + return epoll_ctl(epoll_fd, EPOLL_CTL_DEL, sock->fd, 0); +} + +static int tv_diff(struct timeval *t1, struct timeval *t2) +{ + if (t1->tv_sec != t2->tv_sec) + return (t1->tv_sec - t2->tv_sec) * 1000; + else + return (t1->tv_usec - t2->tv_usec) / 1000; +} + +int uloop_timeout_add(struct uloop_timeout *timeout) +{ + struct uloop_timeout **head = &first_timeout; + struct uloop_timeout *prev = NULL; + + if (timeout->pending) + return -1; + + while (*head) { + if (tv_diff(&(*head)->time, &timeout->time) > 0) + break; + + prev = *head; + head = &(*head)->next; + } + + timeout->prev = prev; + timeout->next = *head; + *head = timeout; + timeout->pending = true; + + return 0; +} + +int uloop_timeout_set(struct uloop_timeout *timeout, int msecs) +{ + struct timeval *time = &timeout->time; + + if (timeout->pending) + uloop_timeout_cancel(timeout); + + gettimeofday(&timeout->time, NULL); + + time->tv_sec += msecs / 1000; + time->tv_usec += msecs % 1000; + + if (time->tv_usec > 1000000) { + time->tv_sec++; + time->tv_usec %= 100000; + } + + return uloop_timeout_add(timeout); +} + +int uloop_timeout_cancel(struct uloop_timeout *timeout) +{ + if (!timeout->pending) + return -1; + + if (timeout->prev) + timeout->prev->next = timeout->next; + else + first_timeout = timeout->next; + + if (timeout->next) + timeout->next->prev = timeout->prev; + + timeout->pending = false; + + return 0; +} + +static void uloop_handle_sigint(int signo) +{ + cancel = true; +} + +static void uloop_setup_signals(void) +{ + struct sigaction s; + memset(&s, 0, sizeof(struct sigaction)); + s.sa_handler = uloop_handle_sigint; + s.sa_flags = 0; + sigaction(SIGINT, &s, NULL); +} + +static int uloop_get_next_timeout(struct timeval *tv) +{ + int diff; + + if (!first_timeout) + return -1; + + diff = tv_diff(&first_timeout->time, tv); + if (diff < 0) + return 0; + + return diff; +} + +static void uloop_process_timeouts(struct timeval *tv) +{ + struct uloop_timeout *timeout; + + while (first_timeout) { + if (tv_diff(&first_timeout->time, tv) > 0) + break; + + timeout = first_timeout; + uloop_timeout_cancel(timeout); + if (timeout->cb) + timeout->cb(timeout); + } +} + +void uloop_end(void) +{ + cancel = true; +} + +int uloop_init(void) +{ + epoll_fd = epoll_create(32); + if (epoll_fd < 0) + return -1; + + fcntl(epoll_fd, F_SETFD, fcntl(epoll_fd, F_GETFD) | FD_CLOEXEC); + return 0; +} + +void uloop_run(void) +{ + struct epoll_event events[10]; + struct timeval tv; + int timeout; + int nfds, n; + + uloop_setup_signals(); + while(!cancel) + { + gettimeofday(&tv, NULL); + uloop_process_timeouts(&tv); + timeout = uloop_get_next_timeout(&tv); + nfds = epoll_wait(epoll_fd, events, ARRAY_SIZE(events), timeout); + for(n = 0; n < nfds; ++n) + { + struct uloop_fd *u = events[n].data.ptr; + unsigned int ev = 0; + + if(events[n].events & EPOLLERR) { + u->error = true; + uloop_fd_delete(u); + } + + if(!(events[n].events & (EPOLLRDHUP|EPOLLIN|EPOLLOUT|EPOLLERR))) + continue; + + if(events[n].events & EPOLLRDHUP) + u->eof = true; + + if(events[n].events & EPOLLIN) + ev |= ULOOP_READ; + + if(events[n].events & EPOLLOUT) + ev |= ULOOP_WRITE; + + if(u->cb) + u->cb(u, ev); + } + } +} + +void uloop_done(void) +{ + close(epoll_fd); +} diff --git a/uloop.h b/uloop.h new file mode 100644 index 0000000..f84050a --- /dev/null +++ b/uloop.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2010 Felix Fietkau + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA. + * + */ + +#ifndef _ULOOP_H__ +#define _ULOOP_H__ + +#include +#include + +struct uloop_fd; +struct uloop_timeout; + +typedef void (*uloop_fd_handler)(struct uloop_fd *u, unsigned int events); +typedef void (*uloop_timeout_handler)(struct uloop_timeout *t); + +#define ULOOP_READ (1 << 0) +#define ULOOP_WRITE (1 << 1) +#define ULOOP_EDGE_TRIGGER (1 << 2) + +struct uloop_fd +{ + uloop_fd_handler cb; + int fd; + bool eof; + bool error; + bool registered; +}; + +struct uloop_timeout +{ + uloop_timeout_handler cb; + struct uloop_timeout *prev; + struct uloop_timeout *next; + struct timeval time; + bool pending; +}; + +int uloop_fd_add(struct uloop_fd *sock, unsigned int flags); +int uloop_fd_delete(struct uloop_fd *sock); + +int uloop_timeout_add(struct uloop_timeout *timeout); +int uloop_timeout_set(struct uloop_timeout *timeout, int msecs); +int uloop_timeout_cancel(struct uloop_timeout *timeout); + +void uloop_end(void); +int uloop_init(void); +void uloop_run(void); +void uloop_done(void); + +#endif -- 2.25.1