static int proto_arp_process(void *proto_priv, struct packet *p, struct proto_process_stack *stack, unsigned int stack_index) {
struct proto_process_stack *s = &stack[stack_index];
if (sizeof(struct arp_packet) > s->plen)
return PROTO_INVALID;
struct arp_packet *apkt = s->pload;
if (ntohs(apkt->hw_type) != 0x1)
// We only support arp for ethernet links for now
return PROTO_INVALID;
if (ntohs(apkt->proto_type) != 0x0800)
// We only support arp for IPv4 addresses
return PROTO_INVALID;
if (apkt->hw_addr_len != 6)
// Ethernet addresses are 6 bytes long
return PROTO_INVALID;
if (apkt->proto_addr_len != 4)
// IPv4 addresses are 4 bytes long
return PROTO_INVALID;
PTYPE_UINT16_SETVAL(s->pkt_info->fields_value[proto_arp_field_oper], ntohs(apkt->oper));
PTYPE_MAC_SETADDR(s->pkt_info->fields_value[proto_arp_field_sender_hw_addr], apkt->sender_hw_addr);
PTYPE_IPV4_SETADDR(s->pkt_info->fields_value[proto_arp_field_sender_proto_addr], apkt->sender_proto_addr);
PTYPE_MAC_SETADDR(s->pkt_info->fields_value[proto_arp_field_target_hw_addr], apkt->target_hw_addr);
PTYPE_IPV4_SETADDR(s->pkt_info->fields_value[proto_arp_field_target_proto_addr], apkt->target_proto_addr);
return PROTO_OK;
}
static int analyzer_docsis_reg_status_update(struct analyzer_docsis_priv *priv, struct analyzer_docsis_cm *cm, enum docsis_mmt_rng_status new_status, ptime ts, struct proto_process_stack *stack, unsigned int stack_index) {
if (cm->ranging_status == new_status)
return POM_OK;
if (event_has_listener(priv->evt_cm_reg_status)) {
struct event *evt = event_alloc(priv->evt_cm_reg_status);
if (!evt) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
struct data *evt_data = event_get_data(evt);
PTYPE_UINT8_SETVAL(evt_data[analyzer_docsis_cm_reg_status_old].value, cm->ranging_status);
data_set(evt_data[analyzer_docsis_cm_reg_status_old]);
PTYPE_UINT8_SETVAL(evt_data[analyzer_docsis_cm_reg_status_new].value, new_status);
data_set(evt_data[analyzer_docsis_cm_reg_status_new]);
PTYPE_MAC_SETADDR(evt_data[analyzer_docsis_cm_reg_status_mac].value, cm->mac);
data_set(evt_data[analyzer_docsis_cm_reg_status_mac]);
PTYPE_UINT8_SETVAL(evt_data[analyzer_docsis_cm_reg_status_timeout].value, T4_TIMEOUT * cm->t4_multiplier);
data_set(evt_data[analyzer_docsis_cm_reg_status_timeout]);
if (event_process(evt, stack, stack_index, ts) != POM_OK) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
}
cm->ranging_status = new_status;
return POM_OK;
}
static int analyzer_docsis_pkt_process(void *obj, struct packet *p, struct proto_process_stack *stack, unsigned int stack_index) {
struct analyzer *analyzer = obj;
struct analyzer_docsis_priv *priv = analyzer->priv;
struct proto_process_stack *s = &stack[stack_index];
uint8_t *type = PTYPE_UINT8_GETVAL(s->pkt_info->fields_value[proto_docsis_mgmt_field_type]);
char *mac_dst = PTYPE_MAC_GETADDR(s->pkt_info->fields_value[proto_docsis_mgmt_field_dst]);
// FIXME : improve this filtering at the source
// Filter some useless messages we don't care about
if (*type == MMT_UCD2 || *type == MMT_UCD3 || *type == MMT_MDD)
return POM_OK;
if (*type != MMT_RNG_RSP) {
pomlog(POMLOG_DEBUG "Unhandled DOCSIS MGMT message type %u for destination mac %02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX", *type, mac_dst[0], mac_dst[1], mac_dst[2], mac_dst[3], mac_dst[4], mac_dst[5]);
return POM_OK;
}
// Use the last bits for the modem ID
uint16_t id = ntohs(*(uint16_t*) (mac_dst + 4)) & ANALYZER_DOCSIS_CM_MASK;
pom_mutex_lock(&priv->lock);
struct analyzer_docsis_cm *cm;
for (cm = priv->cms[id]; cm; cm = cm->next) {
if (!memcmp(cm->mac, mac_dst, sizeof(cm->mac)))
break;
}
if (!cm) {
// Cable modem not found !
cm = malloc(sizeof(struct analyzer_docsis_cm));
if (!cm) {
pom_mutex_unlock(&priv->lock);
pom_oom(sizeof(struct analyzer_docsis_cm));
return POM_ERR;
}
memset(cm, 0, sizeof(struct analyzer_docsis_cm));
cm->t = timer_alloc(cm, analyzer_docsis_cm_timeout);
if (!cm->t) {
pom_mutex_unlock(&priv->lock);
free(cm);
return POM_ERR;
}
cm->analyzer = analyzer;
memcpy(cm->mac, mac_dst, sizeof(cm->mac));
cm->t4_multiplier = 1;
cm->next = priv->cms[id];
if (cm->next)
cm->next->prev = cm;
priv->cms[id] = cm;
// Announce the new CM
if (event_has_listener(priv->evt_cm_new)) {
struct event *evt = event_alloc(priv->evt_cm_new);
if (!evt) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
struct data *evt_data = event_get_data(evt);
PTYPE_MAC_SETADDR(evt_data[analyzer_docsis_cm_new_mac].value, cm->mac);
data_set(evt_data[analyzer_docsis_cm_new_mac]);
PTYPE_STRING_SETVAL(evt_data[analyzer_docsis_cm_new_input].value, p->input->name);
data_set(evt_data[analyzer_docsis_cm_new_input]);
if (event_process(evt, stack, stack_index, p->ts) != POM_OK) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
}
}
switch (*type) {
case MMT_RNG_RSP:
analyzer_docsis_pkt_parse_rng_rsp(priv, cm, p, stack, stack_index);
break;
// FIXME If ranging_status is 0 and we receive another msg, probably it's actually registered
// and we need to call analyzer_docsis_reg_status_update();
}
timer_queue_now(cm->t, T4_TIMEOUT * cm->t4_multiplier, p->ts);
pom_mutex_unlock(&priv->lock);
return POM_OK;
}
static int analyzer_arp_pkt_process(void *obj, struct packet *p, struct proto_process_stack *stack, unsigned int stack_index) {
struct analyzer *analyzer = obj;
struct analyzer_arp_priv *priv = analyzer->priv;
struct proto_process_stack *s = &stack[stack_index];
struct proto_process_stack *s_prev = &stack[stack_index - 1];
struct in_addr arp_ip = PTYPE_IPV4_GETADDR(s->pkt_info->fields_value[proto_arp_field_sender_proto_addr]);
// Discard bogon 0.0.0.0
if (!arp_ip.s_addr)
return POM_OK;
// Find that IP in the table
uint32_t id = arp_ip.s_addr & ANALYZER_ARP_HOST_MASK;
char *arp_mac = PTYPE_MAC_GETADDR(s->pkt_info->fields_value[proto_arp_field_sender_hw_addr]);
uint16_t vlan = 0;
if (s_prev->proto == priv->proto_vlan)
vlan = *PTYPE_UINT16_GETVAL(s_prev->pkt_info->fields_value[proto_vlan_field_vid]);
pom_mutex_lock(&priv->lock);
struct analyzer_arp_host *host;
for (host = priv->hosts[id]; host; host = host->next) {
if (host->ip.s_addr == arp_ip.s_addr && host->vlan == vlan)
break;
}
if (!host) {
// Host not found !
host = malloc(sizeof(struct analyzer_arp_host));
if (!host) {
pom_mutex_unlock(&priv->lock);
pom_oom(sizeof(struct analyzer_arp_host));
return POM_ERR;
}
memset(host, 0, sizeof(struct analyzer_arp_host));
host->ip.s_addr = arp_ip.s_addr;
memcpy(host->mac, arp_mac, sizeof(host->mac));
host->vlan = vlan;
host->next = priv->hosts[id];
if (host->next)
host->next->prev = host;
priv->hosts[id] = host;
pom_mutex_unlock(&priv->lock);
// Announce the new station
if (event_has_listener(priv->evt_new_sta)) {
struct event *evt = event_alloc(priv->evt_new_sta);
if (!evt)
return POM_ERR;
struct data *evt_data = evt->data;
ptype_copy(evt_data[analyzer_arp_new_sta_mac_addr].value, s->pkt_info->fields_value[proto_arp_field_sender_hw_addr]);
data_set(evt_data[analyzer_arp_new_sta_mac_addr]);
ptype_copy(evt_data[analyzer_arp_new_sta_ip_addr].value, s->pkt_info->fields_value[proto_arp_field_sender_proto_addr]);
data_set(evt_data[analyzer_arp_new_sta_ip_addr]);
PTYPE_UINT16_SETVAL(evt_data[analyzer_arp_new_sta_vlan].value, vlan);
data_set(evt_data[analyzer_arp_new_sta_vlan]);
PTYPE_STRING_SETVAL(evt_data[analyzer_arp_new_sta_input].value, p->input->name);
data_set(evt_data[analyzer_arp_new_sta_input]);
if (event_process(evt, stack, stack_index) != POM_OK)
return POM_ERR;
}
// Nothing else to do
return POM_OK;
}
// Host was found, check mac
if (memcmp(host->mac, arp_mac, sizeof(host->mac))) {
if (event_has_listener(priv->evt_sta_changed)) {
struct event *evt = event_alloc(priv->evt_sta_changed);
if (!evt) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
struct data *evt_data = evt->data;
PTYPE_MAC_SETADDR(evt_data[analyzer_arp_sta_changed_old_mac_addr].value, host->mac);
data_set(evt_data[analyzer_arp_sta_changed_old_mac_addr]);
ptype_copy(evt_data[analyzer_arp_sta_changed_new_mac_addr].value, s->pkt_info->fields_value[proto_arp_field_sender_hw_addr]);
data_set(evt_data[analyzer_arp_sta_changed_new_mac_addr]);
ptype_copy(evt_data[analyzer_arp_sta_changed_ip_addr].value, s->pkt_info->fields_value[proto_arp_field_sender_proto_addr]);
data_set(evt_data[analyzer_arp_sta_changed_ip_addr]);
PTYPE_UINT16_SETVAL(evt_data[analyzer_arp_sta_changed_vlan].value, vlan);
data_set(evt_data[analyzer_arp_sta_changed_vlan]);
PTYPE_STRING_SETVAL(evt_data[analyzer_arp_sta_changed_input].value, p->input->name);
data_set(evt_data[analyzer_arp_sta_changed_input]);
if (event_process(evt, stack, stack_index) != POM_OK) {
pom_mutex_unlock(&priv->lock);
return POM_ERR;
}
}
memcpy(host->mac, arp_mac, sizeof(host->mac));
}
pom_mutex_unlock(&priv->lock);
return POM_OK;
}
