256 lines
6.4 KiB
C
256 lines
6.4 KiB
C
/*
|
|
(C) 2012-13 Digital Devices GmbH.
|
|
|
|
This file is part of the octoserve SAT>IP server.
|
|
|
|
Octoserve 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.
|
|
|
|
Octoserve 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 octoserve. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "octoserve.h"
|
|
|
|
extern uint32_t debug;
|
|
|
|
static uint16_t cs16(uint8_t *p, uint32_t len)
|
|
{
|
|
uint32_t cs;
|
|
|
|
cs = (p[0] << 8) | p[1];
|
|
p += 4;
|
|
len -= 4;
|
|
for (; len > 0; len -= 2, p += 2)
|
|
cs += (p[0] << 8) | p[1];
|
|
if (len)
|
|
cs += p[0];
|
|
|
|
while (cs >> 16)
|
|
cs = (cs & 0xffff) + (cs >> 16);
|
|
return ~cs;
|
|
}
|
|
|
|
void proc_igmp(struct octoserve *os, uint8_t *b, int l, uint8_t *macheader)
|
|
{
|
|
int hl = (b[0] & 0xf) * 4, pl, rl, i, j, ns, al;
|
|
uint8_t cmd, *r, *s, type, *p;
|
|
uint16_t n;
|
|
char sd[64], mc[256], *mac = 0;
|
|
uint16_t cs;
|
|
|
|
if (macheader)
|
|
mac = macheader + 6;
|
|
if (hl + 1 > l)
|
|
return;
|
|
|
|
//dump(b, l);
|
|
sprintf(sd, "%03d.%03d.%03d.%03d -> %03d.%03d.%03d.%03d",
|
|
b[12], b[13], b[14], b[15],
|
|
b[16], b[17], b[18], b[19]);
|
|
|
|
p = b + hl;
|
|
pl = l - hl;
|
|
if (pl < 4)
|
|
return;
|
|
if (cs16(p, pl) != ((p[2] << 8) | p[3])) {
|
|
dbgprintf(DEBUG_IGMP, "IGMP CS error\n");
|
|
if (debug & DEBUG_IGMP)
|
|
dump(b, l);
|
|
return;
|
|
}
|
|
|
|
cmd = b[hl];
|
|
switch (cmd) {
|
|
case 0x11:
|
|
{
|
|
uint32_t a1, a2;
|
|
uint8_t *s;
|
|
|
|
if (pl < 12)
|
|
return;
|
|
dbgprintf(DEBUG_IGMP, "IGMP: query by %s\n", sd);
|
|
s = (uint8_t *) &(((struct sockaddr_in *) &os->ssdp.sadr)->sin_addr);
|
|
a1 = (b[12] << 24) | (b[13] << 16) | (b[14] << 8) | b[15];
|
|
a2 = (s[0] << 24) | (s[1] << 16) | (s[2] << 8) | s[3];
|
|
dbgprintf(DEBUG_IGMP, "%08x < %08x ? %s\n", a1, a2, (a1 < a2) ? "yes" : "no");
|
|
if (a1 < a2) {
|
|
/* somebody else with lower IP is already sending queries */
|
|
os->igmp_mode = 3;
|
|
time(&os->igmp_time);
|
|
os->igmp_tag++;
|
|
os->igmp_timeout = b[hl + 1] / 10 + 1;
|
|
dbgprintf(DEBUG_IGMP, "IGMP slave, tag = %u, timeout = %u\n",
|
|
os->igmp_tag, os->igmp_timeout);
|
|
} else
|
|
dbgprintf(DEBUG_IGMP, "IGMP master, tag = %u, timeout = %u\n",
|
|
os->igmp_tag, os->igmp_timeout);
|
|
break;
|
|
}
|
|
case 0x12:
|
|
if (pl < 8)
|
|
return;
|
|
dbgprintf(DEBUG_IGMP, "IGMPV1: %s : %d.%d.%d.%d\n", sd,
|
|
b[hl + 4], b[hl + 5], b[hl + 6], b[hl + 7]);
|
|
mc_join(os, b + 12, mac, &b[hl + 4]);
|
|
break;
|
|
case 0x16:
|
|
if (pl < 8)
|
|
return;
|
|
dbgprintf(DEBUG_IGMP, "IGMPV2: %s : %d.%d.%d.%d\n", sd,
|
|
b[hl + 4], b[hl + 5], b[hl + 6], b[hl + 7]);
|
|
mc_join(os, b + 12, mac, &b[hl + 4]);
|
|
break;
|
|
case 0x17:
|
|
if (pl < 8)
|
|
return;
|
|
dbgprintf(DEBUG_IGMP, "IGMPL: %s : %d.%d.%d.%d\n", sd,
|
|
b[hl + 4], b[hl + 5], b[hl + 6], b[hl + 7]);
|
|
mc_leave(os, b + 12, &b[hl + 4]);
|
|
break;
|
|
case 0x22:
|
|
if (pl < 8)
|
|
return;
|
|
pl -= 8;
|
|
//dump(b + hl, l - hl);
|
|
n = (b[hl + 6] << 8) | b[hl + 7];
|
|
dbgprintf(DEBUG_IGMP, "IGMPV3: %s, %d records:\n", sd, n);
|
|
for (i = 0, r = &b[hl + 8]; i < n; i++, r += rl, pl -= rl) {
|
|
if (pl < 4)
|
|
return;
|
|
ns = (r[2] << 8) | r[3];
|
|
al = r[1] * 4;
|
|
rl = 8 + ns * 4 + al;
|
|
if (pl < rl)
|
|
return;
|
|
type = r[0];
|
|
dbgprintf(DEBUG_IGMP, "type %d, %d sources, %d aux:\n", type, ns, al);
|
|
if (ns) {
|
|
dbgprintf(DEBUG_IGMP, "cannot handle sources\n");
|
|
continue;
|
|
}
|
|
if (type == 2) {
|
|
dbgprintf(DEBUG_IGMP, "IGMPV3: %s subscribed to %d\n", sd, ns);
|
|
mc_join(os, b + 12, mac, &r[4]);
|
|
}
|
|
if (type == 4) {
|
|
dbgprintf(DEBUG_IGMP, "IGMPV3: %s, change to Exclude %d (Join)\n",
|
|
sd, ns);
|
|
mc_join(os, b + 12, mac, &r[4]);
|
|
}
|
|
if (type == 3) {
|
|
dbgprintf(DEBUG_IGMP, "IGMPV3: %s, change to Include %d (Leave)\n",
|
|
sd, ns);
|
|
mc_leave(os, b + 12, &r[4]);
|
|
}
|
|
//printf("MC: %d.%d.%d.%d\n", r[4], r[5], r[6], r[7]);
|
|
for (j = 0, s = &r[8]; j < ns; j++, s += 4) {
|
|
dbgprintf(DEBUG_IGMP, "%d.%d.%d.%d\n", s[0], s[1], s[2], s[3]);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
//dump(b + hl, l - hl);
|
|
}
|
|
|
|
static void calc_cs(uint8_t *m, uint32_t len)
|
|
{
|
|
uint32_t cs = 0, i;
|
|
|
|
for (i = 0; i < len - 1; i += 2)
|
|
cs += (m[i] << 8) | m[i + 1];
|
|
if (i < len)
|
|
cs += m[i] << 8;
|
|
cs = (cs >> 16) + (cs & 0xffff);
|
|
cs = (cs >> 16) + (cs & 0xffff);
|
|
cs ^= 0xffff;
|
|
m[2] = cs >> 8;
|
|
m[3] = cs & 0xff;
|
|
}
|
|
|
|
void send_igmp_query(struct octoserve *os, uint8_t *group, uint8_t timeout)
|
|
{
|
|
uint8_t msg[] = { 0x11, 0x64, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00,
|
|
0x02, 0x7d, 0x00, 0x00
|
|
};
|
|
struct sockaddr_in insadr;
|
|
uint8_t zero = 0, ra[4] = { 0x94, 0x04, 0x00, 0x00 };
|
|
|
|
memset(&insadr, 0, sizeof(insadr));
|
|
insadr.sin_family = AF_INET;
|
|
insadr.sin_port = IPPROTO_IGMP;
|
|
//insadr.sin_addr.s_addr = inet_addr("224.0.0.22");
|
|
insadr.sin_addr.s_addr = inet_addr("224.0.0.1");
|
|
|
|
if (group) {
|
|
msg[4] = group[0];
|
|
msg[5] = group[1];
|
|
msg[6] = group[2];
|
|
msg[7] = group[3];
|
|
memcpy(&insadr.sin_addr.s_addr, group, 4);
|
|
}
|
|
if (timeout)
|
|
msg[1] = timeout;
|
|
|
|
calc_cs(msg, sizeof(msg));
|
|
|
|
setsockopt(os->igmp_sock, IPPROTO_IP, IP_OPTIONS, ra, sizeof(ra));
|
|
sendto(os->igmp_sock, msg, sizeof(msg), 0, (struct sockaddr *) &insadr, sizeof(insadr));
|
|
dbgprintf(DEBUG_IGMP, "Queried group %03u.%03u.%03u.%03u\n",
|
|
msg[4], msg[5], msg[6], msg[7]);
|
|
}
|
|
|
|
void check_igmp(struct octoserve *os)
|
|
{
|
|
time_t tdiff, t;
|
|
|
|
tdiff = time(&t) - os->igmp_time;
|
|
switch (os->igmp_mode) {
|
|
case 0:
|
|
if (tdiff > 124) {
|
|
os->igmp_timeout = 11;
|
|
dbgprintf(DEBUG_IGMP,
|
|
"%u: IGMP master query, tag = %u, timeout = %u\n",
|
|
t, os->igmp_tag, os->igmp_timeout);
|
|
os->igmp_tag++;
|
|
send_igmp_query(os, 0, 0);
|
|
os->igmp_time = t;
|
|
os->igmp_mode = 1;
|
|
if (os->igmp_robust) {
|
|
os->igmp_robust--;
|
|
os->igmp_time -= 94;
|
|
}
|
|
}
|
|
break;
|
|
case 1:
|
|
if (tdiff > os->igmp_timeout) {
|
|
dbgprintf(DEBUG_IGMP, "%u: IGMP timeout, tag = %u\n", t, os->igmp_tag);
|
|
os->igmp_mode = 0;
|
|
}
|
|
break;
|
|
case 2:
|
|
/* check if query master timed out */
|
|
if (tdiff < 255)
|
|
return;
|
|
/* yes, so we will have to query from now on */
|
|
os->igmp_mode = 0;
|
|
os->igmp_time = t - 94;
|
|
os->igmp_robust = 1;
|
|
break;
|
|
case 3:
|
|
if (tdiff > os->igmp_timeout) {
|
|
dbgprintf(DEBUG_IGMP, "%u: IGMP timeout, tag = %u\n", t, os->igmp_tag);
|
|
os->igmp_mode = 2;
|
|
}
|
|
break;
|
|
}
|
|
}
|