1
0
mirror of https://github.com/DigitalDevices/octonet.git synced 2023-10-10 13:36:52 +02:00
octonet/octoserve/igmp.c
2015-08-31 21:29:53 +02:00

258 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 0
if (debug & DEBUG_IGMP)
dump(b, l);
#endif
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;
mtime(&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 = mtime(&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;
}
}