/* Mis-connector - user-space router and NATP for misconfigured hosts on LAN.
Hash functions.
This file is part of mis-connector.
Copyright (C) 2004,2021 Petr Silhavy <petr.silhavy@yandex.com>
SPDX-License-Identifier: GPL-3.0-or-later
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 3 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 mis-connector. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <net/ethernet.h>
#include "whackparam.h"
/*
* SHM @ 0xA0000000
*
* 0xA0000000 - 0xA0040000 MAC => IP hash - size 0x40000
* 0xA0040000 - 0xA0050000 IP => MAC hash - size 0x10000
* 0xA0050000 - 0xA0051000 bitmap - size 0x1000
* 0xA0051000 - 0xA0151000 entries
*
* 4096 bitmap => 1MB mem
* hash 64k + 64k 131072
* 32 * 16k 524288
*
* = 655360
*/
#define BASE 0xA0000000
#define SHM_SIZE (0x50000 + 0x1000 + 0x100000)
struct sb *
sb_init()
{
key_t key = ftok("/dev/whack", 'W');
int shmid = shmget(key, SHM_SIZE, IPC_CREAT | 0600 );
void *addr ;
struct sb *sb = Malloc( sizeof *sb);
if ( shmid == EOF )
BUG("Can't create shared memmory");
addr = shmat(shmid, (char *)BASE, 0);
if ( addr == (void *)EOF)
BUG("Can't attach shared memmory");
memset( addr, 0, SHM_SIZE);
sb->mac_to_ip = addr ;
sb->ip_to_mac = addr + 0x40000 ;
sb->m = addr + 0x50000 ;
sb->pool = addr + 0x51000 ;
memset(sb->mac_to_ip, 0, SHM_SIZE);
return sb ;
}
/* List funtions were formerly part of glib */
/* GLIB - Library of useful routines for C programming
* Copyright (C) 1995-1997 Peter Mattis, Spencer Kimball and Josh MacDonald
*/
#if 0
static struct hash_entry *
g_slist_last (struct hash_entry *list)
{
if (list)
{
while (list->next)
list = list->next;
}
return list;
}
static struct hash_entry *
g_slist_find_mac (struct hash_entry *list, unsigned char *mac)
{
if (list)
{
do
{
if ( ! memcmp(mac, list->mac, ETH_ALEN) )
return list ;
list = list->next ;
}
while (list) ;
}
return list;
}
static void
g_slist_append ( struct hash_entry *list, struct hash_entry *new_list)
{
struct hash_entry *last;
last = g_slist_last (list);
/* g_assert (last != NULL); */
last->next = new_list;
}
static struct hash_entry *
g_slist_remove_link (struct hash_entry *list,
struct hash_entry *link)
{
struct hash_entry *tmp;
struct hash_entry *prev;
prev = NULL;
tmp = list;
while (tmp)
{
if (tmp == link)
{
if (prev)
prev->next = tmp->next;
if (list == tmp)
list = list->next;
tmp->next = NULL;
break;
}
prev = tmp;
tmp = tmp->next;
}
return list;
}
#endif // 0 ///////////////////////////////////////////////
static char ip_pool[IPMAX] = { [0 ... IPMAX-1 ] = ' ' } ;
static int
ip_alloc()
{
char *p = strchr(ip_pool, ' ');
*p = 'X' ;
return p - ip_pool ;
}
static inline unsigned int
ipkey_get(unsigned int netip)
{
#if 0
unsigned char *p = (void *) &wp->fool_mask ;
DEBUGP(stderr,"[%s] %s Mask: %u.%u.%u.%u\n", ids[id], __func__,
p[0],
p[1],
p[2],
p[3]);
#endif /* 0 */
return ntohl(netip & ~wp->fool_mask) ;
}
static inline unsigned int
mackey_get(unsigned char *mac)
{
return mac[4] * 256 + mac[5] ;
}
/* new net order IP */
static unsigned int
ip_make(struct whack_param *wp )
{
char ipbuf[16] ;
unsigned int n = ip_alloc(), ip , hip ;
ip = htonl( wp->fool_min ) + n + 1; /* net order */
hip = ntohl(ip);
inet_ntop(AF_INET, &hip, ipbuf, sizeof ipbuf);
DEBUGP(stderr,"%s New IP: %s\nFor MAC: ", __func__, ipbuf);
return ip ;
}
void
new_hash_entry(struct ether_header *eh , unsigned char *sip /* src IP */)
{
unsigned char *sha = eh->ether_shost ;
unsigned int key = mackey_get(sha) ;
struct hash_entry *he ;
DEBUGP(stderr, "%s %s ->\n", ids[id], __func__ );
if ( sb->mac_to_ip[key] == NULL )
{
he = bm_alloc0(sb, sizeof *he, __func__ );
if ( he == NULL )
{
BUG("No new entry, bm_alloc returns NULL\n");
/* zap old entries, and try againg FIXME !!! or simple *CRASH* */
return ;
}
DEBUGP(stderr, "%s %s allocated -> new_entry\n", ids[id], __func__ );
goto new_entry ;
}
else
{
he = sb->mac_to_ip[key] ;
if ( he->next == NULL && ! memcmp(he->mac, sha, ETH_ALEN) ) /* entry exists */
{
DEBUGP(stderr,"Entry for MAC exists : ");
print_mac(sha);
DEBUGP(stderr,"\n");
return ;
}
/* handle collision */
if ( g_slist_find_mac(he, sha)) /* entry exists */
{
DEBUGP(stderr, "Entry for MAC exists in list : ");
print_mac(sha);
DEBUGP(stderr, "\n");
return ;
}
else
{
struct hash_entry *new = bm_alloc0(sb, sizeof *new, __func__ );
if ( new == NULL )
{
BUG("No new entry, bm_alloc returns NULL\n");
/* zap old entries, and try againg FIXME !!! or simple *CRASH* */
return ;
}
g_slist_append (he, new);
he = new ;
DEBUGP(stderr, "%s %s MAC doesn't exist -> new -> new_entry\n", ids[id], __func__ );
goto new_entry ;
}
}
return ;
new_entry:
memcpy(he->mac, sha, ETH_ALEN);
memcpy(&he->fool_ip, sip, sizeof he->fool_ip);
he->ip = htonl(ip_make(wp));
{
/* unsigned int ipkey = ntohl(ipkey_get(he->ip)); */
unsigned int ipkey = ipkey_get(he->ip);
DEBUGP(stderr, "%s %s ", ids[id], __func__);
print_mac(sha);
DEBUGP(stderr," Fool IP: %u.%u.%u.%u IPkey %d\n",
sip[0],
sip[1],
sip[2],
sip[3],
ipkey);
if ( ipkey > wp->fool_size )
BUG("IP key %u > hash size %d\n", ipkey, wp->fool_size );
else
DEBUGP(stderr,"%s IP key %u\n", __func__, ipkey );
sb->mac_to_ip[key] = he ;
sb->ip_to_mac[ ipkey ] = he ;
}
}
/* return net order IP */
unsigned int
mac_to_ip(struct ether_header *eh )
{ /* */
unsigned int key = mackey_get(eh->ether_shost) ;
struct hash_entry *he ;
again:
if ( sb->mac_to_ip[key] == NULL )
{
unsigned char *sip ;
u_int16_t et = ntohs(eh->ether_type);
struct iphdr *iph = (void *)eh + sizeof *eh ;
print_mac(eh->ether_shost);
WARNING("No entry for MAC\n");
switch ( et )
{
case 0x0 ... 0x800:
iph = (void *)eh + sizeof *eh ;
sip = (void *) &iph->saddr ;
new_hash_entry(eh, sip);
break ;
default:
BUG("Unknown ethertype %u\n", et);
break ;
}
goto again ;
}
else
{
/* Hash entry for MAC found */
union conv c ;
unsigned int *ma = (void *) eh->ether_shost , *mb ; /* 1st four bytes of MAC */
he = sb->mac_to_ip[key] ;
for ( mb = (void *)he->mac ;
he ;
he = he->next , mb = (void *)he->mac )
if ( *ma == *mb )
{
DEBUGP(stderr, "%s %x == %x\n", ids[id], *ma, *mb );
goto found ;
}
else
DEBUGP(stderr, "%s %x != %x => next\n", ids[id], *ma, *mb );
BUG("End of list reached\n");
found:
c.u = he->ip ;
print_mac(eh->ether_shost);
DEBUGP(stderr,"%s Found IP %u.%u.%u.%u\n", ids[id],
c.c[0],
c.c[1],
c.c[2],
c.c[3]);
return he->ip ;
}
}
struct hash_entry *
ip_to_mac_ip(unsigned int netip)
{
unsigned int ipkey = ipkey_get(netip);
struct hash_entry *he ;
he = sb->ip_to_mac[ipkey] ;
if ( he == NULL )
{
union conv c = { .u = netip } ;
WARNING("No IP entry for %u.%u.%u.%u\n",
c.c[0],
c.c[1],
c.c[2],
c.c[3]);
return NULL ;
}
/* No collisions handling needed here , civilised IP is unique key */
return he ;
}