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; }