static void fix_beacon(struct network * n)
{
REQUIRE(n != NULL);
unsigned char * p;
int ssidlen;
int origlen;
/* beacon surgery */
p = n->n_beacon + sizeof(struct ieee80211_frame) + 8 + 2 + 2;
ssidlen = strlen(n->n_ssid);
ALLEGE((n->n_beaconlen + ssidlen) <= (int) sizeof(n->n_beacon));
ALLEGE(*p == IEEE80211_ELEMID_SSID);
p++;
if (*p != 0 && p[1] != 0) return;
origlen = *p;
*p++ = ssidlen;
ALLEGE(origlen == 0 || p[0] == 0);
memmove(
p + ssidlen, p + origlen, n->n_beaconlen - (p + origlen - n->n_beacon));
memcpy(p, n->n_ssid, ssidlen);
n->n_beaconlen += ssidlen - origlen;
}
static int
handle(int s, unsigned char * data, int len, struct sockaddr_in * s_in)
{
REQUIRE(data != NULL);
REQUIRE(s_in != NULL);
char buf[2048];
unsigned short * cmd = (unsigned short *) buf;
int plen;
struct in_addr * addr = &s_in->sin_addr;
unsigned short * pid = (unsigned short *) data;
/* inet check */
if (len == S_HELLO_LEN && memcmp(data, "sorbo", 5) == 0)
{
unsigned short * id = (unsigned short *) (data + 5);
int x = 2 + 4 + 2;
*cmd = htons(S_CMD_INET_CHECK);
memcpy(cmd + 1, addr, 4);
memcpy(cmd + 1 + 2, id, 2);
printf("Inet check by %s %d\n", inet_ntoa(*addr), ntohs(*id));
if (send(s, buf, x, 0) != x) return (1);
return (0);
}
*cmd++ = htons(S_CMD_PACKET);
*cmd++ = *pid;
plen = len - 2;
if (plen < 0) return (0);
last_id = ntohs(*pid);
if (last_id > 20000) wrap = 1;
if (wrap && last_id < 100)
{
wrap = 0;
memset(ids, 0, sizeof(ids));
}
printf("Got packet %d %d", last_id, plen);
if (is_dup(last_id))
{
printf(" (DUP)\n");
return (0);
}
printf("\n");
*cmd++ = htons(plen);
memcpy(cmd, data + 2, plen);
plen += 2 + 2 + 2;
ALLEGE(plen <= (int) sizeof(buf));
if (send(s, buf, plen, 0) != plen) return (1);
return (0);
}
static int parse_rsn(const unsigned char * p, const int l, const int rsn)
{
REQUIRE(p != NULL);
int c;
const unsigned char * start = p;
int psk = 0;
int wpa = 0;
if (l < 2) return (0);
if (memcmp(p, "\x01\x00", 2) != 0) return (0);
wpa = 1;
if (l < 8) return (-1);
p += 2;
p += 4;
/* cipher */
c = le16toh(*((uint16_t *) p));
p += 2 + 4 * c;
if (l < ((p - start) + 2)) return (-1);
/* auth */
c = le16toh(*((uint16_t *) p));
p += 2;
if (l < ((p - start) + c * 4)) return (-1);
while (c--)
{
if (rsn && memcmp(p, "\x00\x0f\xac\x02", 4) == 0) psk = 1;
if (!rsn && memcmp(p, "\x00\x50\xf2\x02", 4) == 0) psk = 1;
p += 4;
}
ALLEGE(l >= (p - start));
if (!psk) wpa = 0;
return (wpa);
}
static void save_network(const struct network * n)
{
REQUIRE(n != NULL);
int i;
_outfd = open_pcap(_outfilename);
write_pcap(_outfd, n->n_beacon, n->n_beaconlen);
for (i = 0; i < 4; i++)
{
struct packet * p = &n->n_handshake->c_handshake[i];
ALLEGE(p != NULL); //-V547
if (p->p_len) packet_write_pcap(_outfd, p);
}
}
static void process_beacon(struct ieee80211_frame * wh, int totlen)
{
REQUIRE(wh != NULL);
unsigned char * p = (unsigned char *) (wh + 1);
int bhlen = 8 + 2 + 2;
int len = totlen;
char ssid[256];
int wpa = 0;
int rc;
int ssids = 0;
int hidden = 0;
struct network * n;
totlen -= sizeof(*wh);
if (totlen < bhlen) goto __bad;
if (!(IEEE80211_BEACON_CAPABILITY(p) & IEEE80211_CAPINFO_PRIVACY)) return;
p += bhlen;
totlen -= bhlen;
ssid[0] = 0;
while (totlen > 2)
{
int id = *p++;
int l = *p++;
totlen -= 2;
if (totlen < l) goto __bad;
switch (id)
{
case IEEE80211_ELEMID_SSID:
if (++ssids > 1) break;
if (l == 0 || p[0] == 0)
hidden = 1;
else
{
memcpy(ssid, p, l);
ssid[l] = 0;
}
break;
case IEEE80211_ELEMID_VENDOR:
if ((rc = parse_elem_vendor(&p[-2], l + 2)) == -1) goto __bad;
if (rc) wpa = 1;
break;
case IEEE80211_ELEMID_RSN:
if ((rc = parse_rsn(p, l, 1)) == -1) goto __bad;
if (rc) wpa = 1;
break;
}
p += l;
totlen -= l;
}
if (!wpa) return;
n = find_add_net(wh->i_addr3);
if (n->n_beaconlen) return;
n->n_beaconlen = len;
ALLEGE(n->n_beaconlen <= (int) sizeof(n->n_beacon));
memcpy(n->n_beacon, wh, n->n_beaconlen);
strncpy(n->n_ssid, ssid, sizeof(n->n_ssid));
(n->n_ssid)[sizeof(n->n_ssid) - 1] = '\0';
check_network(n);
return;
__bad:
printf("bad beacon\n");
}
static int _remove(c_avl_tree_t * t, c_avl_node_t * n)
{
assert((t != NULL) && (n != NULL));
if ((n->left != NULL) && (n->right != NULL))
{
c_avl_node_t * r; /* replacement node */
if (BALANCE(n) > 0) /* left subtree is higher */
{
assert(n->left != NULL); //-V547
r = c_avl_node_prev(n);
}
else /* right subtree is higher */
{
assert(n->right != NULL); //-V547
r = c_avl_node_next(n);
}
ALLEGE(r != NULL && ((r->left == NULL) || (r->right == NULL)));
/* copy content */
n->key = r->key;
n->value = r->value;
n = r;
}
ALLEGE(n != NULL && ((n->left == NULL) || (n->right == NULL)));
if ((n->left == NULL) && (n->right == NULL))
{
/* Deleting a leave is easy */
if (n->parent == NULL)
{
assert(t->root == n);
t->root = NULL;
}
else
{
assert((n->parent->left == n) || (n->parent->right == n));
if (n->parent->left == n)
n->parent->left = NULL;
else
n->parent->right = NULL;
rebalance(t, n->parent);
}
free_node(n);
}
else if (n->left == NULL)
{
assert(BALANCE(n) == -1); //-V547
assert((n->parent == NULL) || (n->parent->left == n)
|| (n->parent->right == n));
if (n->parent == NULL)
{
assert(t->root == n);
t->root = n->right;
}
else if (n->parent->left == n)
{
n->parent->left = n->right;
}
else
{
n->parent->right = n->right;
}
ALLEGE(n->right != NULL);
n->right->parent = n->parent;
if (n->parent != NULL) rebalance(t, n->parent);
n->right = NULL;
free_node(n);
}
else if (n->right == NULL)
{
assert(BALANCE(n) == 1); //-V547
assert((n->parent == NULL) || (n->parent->left == n)
|| (n->parent->right == n));
if (n->parent == NULL)
{
assert(t->root == n);
t->root = n->left;
}
else if (n->parent->left == n)
{
n->parent->left = n->left;
}
else
{
n->parent->right = n->left;
}
n->left->parent = n->parent;
if (n->parent != NULL) rebalance(t, n->parent);
n->left = NULL;
free_node(n);
}
else
{
assert(0);
}
return 0;
} /* void *_remove */
