static void dissect_igmp_v2(struct pkt_buff *pkt)
{
char addr[INET_ADDRSTRLEN];
uint16_t csum;
struct igmp_v2_msg *msg =
(struct igmp_v2_msg *) pkt_pull(pkt, sizeof(*msg));
if (msg == NULL)
return;
switch (msg->type) {
case RGMP_HELLO:
case RGMP_BYE:
case RGMP_JOIN_GROUP:
case RGMP_LEAVE_GROUP:
tprintf(" [ IGMPv2 (RGMP)");
break;
default:
tprintf(" [ IGMPv2");
break;
}
PRINT_FRIENDLY_NAMED_MSG_TYPE(msg->type);
tprintf(", Max Resp Time (%u)", msg->max_resp_time);
csum = calc_csum(msg, sizeof(*msg) + pkt_len(pkt), 0);
tprintf(", CSum (0x%.4x) is %s", ntohs(msg->checksum), csum ?
colorize_start_full(black, red) "bogus (!)" colorize_end() : "ok");
if (csum)
tprintf(" - %s should be %x%s", colorize_start_full(black, red),
csum_expected(msg->checksum, csum), colorize_end());
inet_ntop(AF_INET, &msg->group_address, addr, sizeof(addr));
tprintf(", Group Addr (%s)", addr);
tprintf(" ]\n");
}
static void apply_csum16(int csum_id)
{
int j, i = csum_id;
size_t csum_max = packet_dyn[i].slen;
for (j = 0; j < csum_max; ++j) {
uint16_t sum = 0;
struct csum16 *csum = &packet_dyn[i].csum[j];
fmemset(&packets[i].payload[csum->off], 0, sizeof(sum));
switch (csum->which) {
case CSUM_IP:
if (csum->to >= packets[i].len)
csum->to = packets[i].len - 1;
sum = calc_csum(packets[i].payload + csum->from,
csum->to - csum->from + 1, 0);
break;
case CSUM_UDP:
sum = p4_csum((void *) packets[i].payload + csum->from,
packets[i].payload + csum->to,
(packets[i].len - csum->to),
IPPROTO_UDP);
break;
case CSUM_TCP:
sum = p4_csum((void *) packets[i].payload + csum->from,
packets[i].payload + csum->to,
(packets[i].len - csum->to),
IPPROTO_TCP);
break;
}
fmemcpy(&packets[i].payload[csum->off], &sum, sizeof(sum));
}
}
static void dissect_igmp_v3_membership_query(struct pkt_buff *pkt)
{
char addr[INET_ADDRSTRLEN];
size_t n;
uint16_t csum;
uint32_t *src_addr;
struct igmp_v3_membership_query *msg =
(struct igmp_v3_membership_query *) pkt_pull(pkt, sizeof(*msg));
if (msg == NULL)
return;
tprintf(" [ IGMPv3");
PRINT_FRIENDLY_NAMED_MSG_TYPE(msg->type);
tprintf(", Max Resp Code (0x%.2x => %u)", msg->max_resp_code,
DECODE_MAX_RESP_CODE(msg->max_resp_code));
csum = calc_csum(msg, sizeof(*msg) + pkt_len(pkt), 0);
tprintf(", CSum (0x%.4x) is %s", ntohs(msg->checksum), csum ?
colorize_start_full(black, red) "bogus (!)" colorize_end() : "ok");
if (csum)
tprintf(" - %s should be %x%s", colorize_start_full(black, red),
csum_expected(msg->checksum, csum), colorize_end());
inet_ntop(AF_INET, &msg->group_address, addr, sizeof(addr));
/* S Flag (Suppress Router-Side Processing) */
tprintf(", Suppress (%u)", msg->s_flag ? 1 : 0);
/* QRV (Querier's Robustness Variable) */
tprintf(", QRV (%u)", msg->qrv);
/* QQIC (Querier's Query Interval Code) */
tprintf(", QQIC (0x%.2x => %u)", msg->qqic, DECODE_QQIC(msg->qqic));
tprintf(", Group Addr (%s)", addr);
n = ntohs(msg->number_of_sources);
tprintf(", Num Src (%zu)", n);
if (n--) {
src_addr = (uint32_t *) pkt_pull(pkt, sizeof(*src_addr));
if (src_addr != NULL) {
inet_ntop(AF_INET, src_addr, addr, sizeof(addr));
tprintf(", Src Addr (%s", addr);
while (n--) {
src_addr = (uint32_t *)
pkt_pull(pkt, sizeof(*src_addr));
if (src_addr == NULL)
break;
inet_ntop(AF_INET, src_addr, addr, sizeof(addr));
tprintf(", %s", addr);
}
tprintf(")");
}
}
tprintf(" ]\n");
}
int check_cubie_header(const char *devpath)
{
struct cubie_hdr hdr;
uint32_t csum;
int fd;
int ret;
int k;
memset(&hdr, 0, sizeof(hdr));
for(k=0; k<3; k++) {
if(!access(devpath, R_OK)) {
ERROR("get block device count=%d", k);
}
sleep(1);
}
if (access(devpath, R_OK)) {
ERROR("Permission denied for %s", devpath);
return 0;
}
fd = open(devpath, O_RDONLY);
if (fd < 0) {
ERROR("Open failed: %s", devpath);
return 0;
}
ret = read(fd, &hdr, sizeof(hdr));
if (ret < 0) {
ERROR("read failed: %s", devpath);
close(fd);
return 0;
}
close(fd);
if (hdr.csum == calc_csum(&hdr)) {
ERROR("check sata filesystem ok\n");
return 1;
} else {
ERROR("check sata filesystem failed\n");
return 0;
}
}
static void ipv4_csum_update(struct proto_hdr *hdr)
{
struct packet *pkt;
uint16_t csum;
if (hdr->is_csum_valid)
return;
if (proto_hdr_field_is_set(hdr, IP4_CSUM))
return;
pkt = packet_get(hdr->pkt_id);
proto_hdr_field_set_default_u16(hdr, IP4_CSUM, 0);
csum = htons(calc_csum(&pkt->payload[hdr->pkt_offset], hdr->len));
proto_hdr_field_set_default_u16(hdr, IP4_CSUM, bswap_16(csum));
hdr->is_csum_valid = true;
}
static void icmpv4_csum_update(struct proto_hdr *hdr)
{
struct packet *pkt;
uint16_t csum;
if (hdr->is_csum_valid)
return;
if (proto_hdr_field_is_set(hdr, ICMPV4_CSUM))
return;
pkt = packet_get(hdr->pkt_id);
proto_hdr_field_set_default_u16(hdr, ICMPV4_CSUM, 0);
csum = htons(calc_csum(proto_header_ptr(hdr), pkt->len - hdr->pkt_offset));
proto_hdr_field_set_default_u16(hdr, ICMPV4_CSUM, bswap_16(csum));
hdr->is_csum_valid = true;
}
int write_cubie_header(const char *devpath)
{
struct cubie_hdr hdr;
uint32_t csum;
int fd;
int ret;
memset(&hdr, 0, sizeof(hdr));
hdr.cubie_magic = CUBIE_MAGIC;
snprintf((char *)hdr.cubie_token, sizeof(hdr.cubie_token), "%s", "cubiesata");
csum = calc_csum(&hdr);
hdr.csum = csum;
// if (access(devpath, W_OK)) {
// ALOGW("Permission denied for %s", devpath);
// return 0;
// }
fd = open(devpath, O_RDWR);
if (fd < 0) {
ALOGW("Open failed: %s", devpath);
return 0;
}
ret = write(fd, &hdr, sizeof(hdr));
if (ret < 0) {
ALOGW("Write failed: %s", devpath);
close(fd);
return 0;
}
close(fd);
sync();
return 1;
}
int check_cubie_header(const char *devpath)
{
struct cubie_hdr hdr;
uint32_t csum;
int fd;
int ret;
memset(&hdr, 0, sizeof(hdr));
if (access(devpath, O_RDONLY)) {
ALOGW("Permission denied for %s", devpath);
return 0;
}
fd = open(devpath, O_RDONLY);
if (fd < 0) {
ALOGW("Open failed: %s", devpath);
return 0;
}
ret = read(fd, &hdr, sizeof(hdr));
if (ret < 0) {
ALOGW("read failed: %s", devpath);
close(fd);
return 0;
}
close(fd);
if (hdr.csum == calc_csum(&hdr)) {
printf("check ok\n");
} else {
printf("check failed\n");
}
return 1;
}
static void dissect_igmp_v1(struct pkt_buff *pkt)
{
char addr[INET_ADDRSTRLEN];
uint16_t csum;
struct igmp_v1_msg *msg =
(struct igmp_v1_msg *) pkt_pull(pkt, sizeof(*msg));
if (msg == NULL)
return;
tprintf(" [ IGMPv1");
PRINT_FRIENDLY_NAMED_MSG_TYPE(msg->version__type);
csum = calc_csum(msg, sizeof(*msg) + pkt_len(pkt), 0);
tprintf(", CSum (0x%.4x) is %s", ntohs(msg->checksum), csum ?
colorize_start_full(black, red) "bogus (!)" colorize_end() : "ok");
if (csum)
tprintf(" - %s should be %x%s", colorize_start_full(black, red),
csum_expected(msg->checksum, csum), colorize_end());
inet_ntop(AF_INET, &msg->group_address, addr, sizeof(addr));
tprintf(", Group Addr (%s)", addr);
tprintf(" ]\n");
}
static void task_combuf_send()
{
switch (out_offset) {
case -1:
combuf_destroy(out_combuf);
++out_offset;
case 0:
out_combuf =
combuf_search(READY_MASK | INPUT_MASK | CREATE_MASK,
READY_MASK | CREATE_MASK);
if (out_combuf < 0)
break;
/* подсчитываем контрольную сумму. Поле можно не защищать,
* поскольку доступ только в этом месте. */
CSUM(out_combuf) = calc_csum(out_combuf);
out_xor_char = 0;
++out_offset;
uart_tx(BEGIN_MARKER);
return;
}
out_offset = -2;
}
//-----------------------------------------------------------------------------
int iCAN_TX( )
{ int i, errsv=0, n=0;
if(sfd)
{ framewrite.can_id = 55;
framewrite.can_dlc = 8;
framewrite.data[1] = 6; // src _this->node
framewrite.data[2] = 9; // dest
framewrite.data[3] = 1; // CMD write-word
framewrite.data[4] = 0x10; // address high
framewrite.data[5] = 0x02; // address low
framewrite.data[6] = 00; // data high
framewrite.data[7] = 00; // data low
framewrite.data[0] = calc_csum(&framewrite.data[1],framewrite.can_dlc-1 );
n = write(sfd, &framewrite, sizeof(struct can_frame) );
if(n <0) // 6 ENXIO No such device or address
{ errsv = errno; //22=EINVAL Invalid argument
printf("\t\t%d %d %d\n",errsv, framewrite.can_dlc, n );
assert(n >0);
}
}
return(n);
}
static void ipv4(struct pkt_buff *pkt)
{
uint16_t csum, frag_off, h_tot_len;
char src_ip[INET_ADDRSTRLEN];
char dst_ip[INET_ADDRSTRLEN];
struct ipv4hdr *ip = (struct ipv4hdr *) pkt_pull(pkt, sizeof(*ip));
uint8_t *opt, *trailer;
unsigned int trailer_len = 0;
ssize_t opts_len, opt_len;
struct sockaddr_in sas, sad;
const char *city, *region, *country;
if (!ip)
return;
frag_off = ntohs(ip->h_frag_off);
h_tot_len = ntohs(ip->h_tot_len);
csum = calc_csum(ip, ip->h_ihl * 4, 0);
inet_ntop(AF_INET, &ip->h_saddr, src_ip, sizeof(src_ip));
inet_ntop(AF_INET, &ip->h_daddr, dst_ip, sizeof(dst_ip));
if ((pkt_len(pkt) + sizeof(*ip)) > h_tot_len) {
trailer_len = pkt_len(pkt) + sizeof(*ip) - h_tot_len;
trailer = pkt->data + h_tot_len + trailer_len;
}
if (trailer_len) {
tprintf(" [ Eth trailer ");
while (trailer_len--) {
tprintf("%x", *(trailer - trailer_len));
}
tprintf(" ]\n");
}
tprintf(" [ IPv4 ");
tprintf("Addr (%s => %s), ", src_ip, dst_ip);
tprintf("Proto (%u), ", ip->h_protocol);
tprintf("TTL (%u), ", ip->h_ttl);
tprintf("TOS (%u), ", ip->h_tos);
tprintf("Ver (%u), ", ip->h_version);
tprintf("IHL (%u), ", ip->h_ihl);
tprintf("Tlen (%u), ", ntohs(ip->h_tot_len));
tprintf("ID (%u), ", ntohs(ip->h_id));
tprintf("Res (%u), NoFrag (%u), MoreFrag (%u), FragOff (%u), ",
FRAG_OFF_RESERVED_FLAG(frag_off) ? 1 : 0,
FRAG_OFF_NO_FRAGMENT_FLAG(frag_off) ? 1 : 0,
FRAG_OFF_MORE_FRAGMENT_FLAG(frag_off) ? 1 : 0,
FRAG_OFF_FRAGMENT_OFFSET(frag_off));
tprintf("CSum (0x%.4x) is %s", ntohs(ip->h_check),
csum ? colorize_start_full(black, red) "bogus (!)"
colorize_end() : "ok");
if (csum)
tprintf("%s should be 0x%.4x%s", colorize_start_full(black, red),
csum_expected(ip->h_check, csum), colorize_end());
tprintf(" ]\n");
memset(&sas, 0, sizeof(sas));
sas.sin_family = PF_INET;
sas.sin_addr.s_addr = ip->h_saddr;
memset(&sad, 0, sizeof(sad));
sad.sin_family = PF_INET;
sad.sin_addr.s_addr = ip->h_daddr;
if (geoip_working()) {
tprintf("\t[ Geo (");
if ((country = geoip4_country_name(sas))) {
tprintf("%s", country);
if ((region = geoip4_region_name(sas)))
tprintf(" / %s", region);
if ((city = geoip4_city_name(sas)))
tprintf(" / %s", city);
} else {
tprintf("local");
}
tprintf(" => ");
if ((country = geoip4_country_name(sad))) {
tprintf("%s", country);
if ((region = geoip4_region_name(sad)))
tprintf(" / %s", region);
if ((city = geoip4_city_name(sad)))
tprintf(" / %s", city);
} else {
tprintf("local");
}
tprintf(") ]\n");
}
opts_len = max((uint8_t) ip->h_ihl, sizeof(*ip) / sizeof(uint32_t)) *
sizeof(uint32_t) - sizeof(*ip);
for (opt = pkt_pull(pkt, opts_len); opt && opts_len > 0; opt++) {
tprintf(" [ Option Copied (%u), Class (%u), Number (%u)",
IP_OPT_COPIED_FLAG(*opt) ? 1 : 0, IP_OPT_CLASS(*opt),
IP_OPT_NUMBER(*opt));
switch (*opt) {
case IP_OPT_EOOL:
case IP_OPT_NOP:
tprintf(" ]\n");
opts_len--;
break;
default:
/*
* Assuming that EOOL and NOP are the only single-byte
* options, treat all other options as variable in
* length with a minimum of 2.
*
* TODO: option length might be incorrect in malformed packets,
* check and handle that
*/
opt_len = *(++opt);
if (opt_len > opts_len) {
tprintf(", Len (%zd, invalid) ]\n", opt_len);
goto out;
} else
tprintf(", Len (%zd) ]\n", opt_len);
opts_len -= opt_len;
tprintf(" [ Data hex ");
for (opt_len -= 2; opt_len > 0; opt_len--)
tprintf(" %.2x", *(++opt));
tprintf(" ]\n");
break;
}
}
out:
/* cut off everything that is not part of IPv4 payload */
/* XXX there could still be an Ethernet trailer included or others */
pkt_trim(pkt, pkt_len(pkt) - min(pkt_len(pkt),
(ntohs(ip->h_tot_len) - ip->h_ihl * sizeof(uint32_t))));
pkt_set_proto(pkt, ð_lay3, ip->h_protocol);
}
static void dissect_igmp_v3_membership_report(struct pkt_buff *pkt)
{
char addr[INET_ADDRSTRLEN];
size_t m, n;
uint16_t csum;
uint32_t *src_addr;
struct igmp_v3_group_record *rec;
struct igmp_v3_membership_report *msg =
(struct igmp_v3_membership_report *) pkt_pull(pkt, sizeof(*msg));
if (msg == NULL)
return;
tprintf(" [ IGMPv3");
PRINT_FRIENDLY_NAMED_MSG_TYPE(msg->type);
csum = calc_csum(msg, sizeof(*msg) + pkt_len(pkt), 0);
tprintf(", CSum (0x%.4x) is %s", ntohs(msg->checksum), csum ?
colorize_start_full(black, red) "bogus (!)" colorize_end() : "ok");
if (csum)
tprintf(" - %s should be %x%s", colorize_start_full(black, red),
csum_expected(msg->checksum, csum), colorize_end());
m = ntohs(msg->number_of_group_records);
tprintf(", Num Group Rec (%zu)", m);
tprintf(" ]\n");
while (m--) {
rec = (struct igmp_v3_group_record *) pkt_pull(pkt, sizeof(*rec));
if (rec == NULL)
break;
tprintf(" [ Group Record");
if (friendly_group_rec_type_name(rec->record_type))
tprintf(" Type (%u, %s)", rec->record_type,
friendly_group_rec_type_name(rec->record_type));
else
tprintf(" Type (%u)", rec->record_type);
n = ntohs(rec->number_of_sources);
tprintf(", Num Src (%zu)", n);
inet_ntop(AF_INET, &rec->multicast_address, addr, sizeof(addr));
tprintf(", Multicast Addr (%s)", addr);
if (n--) {
src_addr = (uint32_t *) pkt_pull(pkt, sizeof(*src_addr));
if (src_addr != NULL) {
inet_ntop(AF_INET, src_addr, addr, sizeof(addr));
tprintf(", Src Addr (%s", addr);
while (n--) {
src_addr = (uint32_t *)
pkt_pull(pkt, sizeof(*src_addr));
if (src_addr == NULL)
break;
inet_ntop(AF_INET, src_addr, addr, sizeof(addr));
tprintf(", %s", addr);
}
tprintf(")");
}
}
tprintf(" ]\n");
}
tprintf("\n");
}
static void dissect_igmp_v0(struct pkt_buff *pkt)
{
char addr[INET_ADDRSTRLEN];
uint16_t csum;
static const char *reply_codes[] = {
"Request Granted",
"Request Denied, No Resources",
"Request Denied, Invalid Code",
"Request Denied, Invalid Group Address",
"Request Denied, Invalid Access Key"
};
struct igmp_v0_msg *msg =
(struct igmp_v0_msg *) pkt_pull(pkt, sizeof(*msg));
if (msg == NULL)
return;
tprintf(" [ IGMPv0");
PRINT_FRIENDLY_NAMED_MSG_TYPE(msg->type);
switch (msg->type) {
case IGMP_V0_CREATE_GROUP_REQUEST:
switch (msg->code) {
case 0:
tprintf(", Code (%u, %s)", msg->code, "Public");
break;
case 1:
tprintf(", Code (%u, %s)", msg->code, "Private");
break;
default:
tprintf(", Code (%u)", msg->code);
}
break;
case IGMP_V0_CREATE_GROUP_REPLY:
case IGMP_V0_JOIN_GROUP_REPLY:
case IGMP_V0_LEAVE_GROUP_REPLY:
case IGMP_V0_CONFIRM_GROUP_REPLY:
if (msg->code < 5)
tprintf(", Code (%u, %s)", msg->code, reply_codes[msg->code]);
else
tprintf(", Code (%u, Request Pending, Retry In %u Seconds)",
msg->code, msg->code);
break;
default:
tprintf(", Code (%u)", msg->code);
}
csum = calc_csum(msg, sizeof(*msg) + pkt_len(pkt), 0);
tprintf(", CSum (0x%.4x) is %s", ntohs(msg->checksum), csum ?
colorize_start_full(black, red) "bogus (!)" colorize_end() : "ok");
if (csum)
tprintf(" - %s should be %x%s", colorize_start_full(black, red),
csum_expected(msg->checksum, csum), colorize_end());
tprintf(", Id (%u)", ntohs(msg->identifier));
inet_ntop(AF_INET, &msg->group_address, addr, sizeof(addr));
tprintf(", Group Addr (%s)", addr);
tprintf(", Access Key (0x%.16lx)", msg->access_key);
tprintf(" ]\n");
}
