void LrDpleToDple::init() {
// Initialize the base classes
LrDpleInternal::init();
MX As = MX::sym("As", input(LR_DPLE_A).sparsity());
MX Vs = MX::sym("Vs", input(LR_DPLE_V).sparsity());
MX Cs = MX::sym("Cs", input(LR_DPLE_C).sparsity());
MX Hs = MX::sym("Hs", input(LR_DPLE_H).sparsity());
int n_ = A_[0].size1();
// Chop-up the arguments
std::vector<MX> As_ = horzsplit(As, n_);
std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2());
std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2());
std::vector<MX> Hss_ = horzsplit(Hs, Hsi_);
std::vector<MX> V_(Vs_.size());
for (int k=0;k<V_.size();++k) {
V_[k] = mul(Cs_[k], mul(Vs_[k], Cs_[k].T()));
}
std::vector<Sparsity> Vsp(Vs_.size());
for (int k=0;k<V_.size();++k) {
Vsp[k] = V_[k].sparsity();
}
// Solver options
Dict options;
if (hasSetOption(optionsname())) {
options = getOption(optionsname());
}
// Create an dplesolver instance
std::map<std::string, std::vector<Sparsity> > tmp;
tmp["a"] = A_;
tmp["v"] = Vsp;
solver_ = DpleSolver("solver", getOption(solvername()), tmp, options);
MX P = solver_(make_map("a", horzcat(As_), "v", horzcat(V_))).at("p");
std::vector<MX> Ps_ = horzsplit(P, n_);
std::vector<MX> HPH(K_);
for (int k=0;k<K_;++k) {
std::vector<MX> hph = horzsplit(Hss_[k], cumsum0(Hs_[k]));
for (int kk=0;kk<hph.size();++kk) {
hph[kk] = mul(hph[kk].T(), mul(Ps_[k], hph[kk]));
}
HPH[k] = diagcat(hph);
}
f_ = MXFunction(name_, lrdpleIn("a", As, "v", Vs, "c", Cs, "h", Hs),
lrdpleOut("y", horzcat(HPH)));
Wrapper<LrDpleToDple>::checkDimensions();
}
void FixedSmithLrDleInternal::init() {
iter_ = getOption("iter");
LrDleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
MX H = MX::sym("H", H_);
MX A = MX::sym("A", A_);
MX C = MX::sym("C", C_);
MX V = MX::sym("V", V_);
MX Vs = (V+V.T())/2;
MX D = with_C_ ? C : DMatrix::eye(A_.size1());
std::vector<MX> HPH(Hs_.size(), 0);
std::vector<MX> Hs = with_H_? horzsplit(H, Hi_) : std::vector<MX>();
MX out = 0;
for (int i=0;i<iter_;++i) {
if (with_H_) {
for (int k=0;k<Hs.size();++k) {
MX v = mul(D.T(), Hs[k]);
HPH[k]+= mul(v.T(), mul(Vs, v));
}
} else {
out += mul(D, mul(Vs, D.T()));
}
D = mul(A, D);
}
std::vector<MX> dle_in(LR_DLE_NUM_IN);
dle_in[LR_DLE_A] = A;
dle_in[LR_DLE_V] = V;
if (with_C_) dle_in[LR_DLE_C] = C;
if (with_H_) dle_in[LR_DLE_H] = H;
f_ = MXFunction(dle_in, lrdleOut("y", with_H_? diagcat(HPH): out));
f_.init();
Wrapper<FixedSmithLrDleInternal>::checkDimensions();
}
inline
#endif
int pcapProcess(sHeaderPacket **header_packet, int pushToStack_queue_index,
pcap_block_store *block_store, int block_store_index,
int ppf,
pcapProcessData *ppd, int pcapLinklayerHeaderType, pcap_dumper_t *pcapDumpHandle, const char *interfaceName) {
pcap_pkthdr_plus2 *pcap_header_plus2 = NULL;
u_char *packet = NULL;
if(header_packet) {
if((*header_packet)->detect_headers & 0x01) {
ppd->header_ip_offset = (*header_packet)->header_ip_first_offset;
ppd->protocol = (*header_packet)->eth_protocol;
ppd->header_ip = (iphdr2*)(HPP(*header_packet) + ppd->header_ip_offset);
} else if(parseEtherHeader(pcapLinklayerHeaderType, HPP(*header_packet),
ppd->header_sll, ppd->header_eth, ppd->header_ip_offset, ppd->protocol)) {
(*header_packet)->detect_headers |= 0x01;
(*header_packet)->header_ip_first_offset = ppd->header_ip_offset;
(*header_packet)->eth_protocol = ppd->protocol;
if(ppd->protocol != ETHERTYPE_IP) {
if(sverb.tcpreplay) {
if(ppd->protocol == 0) {
ppd->header_ip_offset += 2;
ppd->protocol = ETHERTYPE_IP;
} else {
return(0);
}
} else {
pcapProcessEvalError(bad_eth_protocol, *HPH(*header_packet), HPP(*header_packet),
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
return(0);
}
}
ppd->header_ip = (iphdr2*)(HPP(*header_packet) + ppd->header_ip_offset);
if(ppd->header_ip->version != 4) {
pcapProcessEvalError(bad_ip_version, *HPH(*header_packet), HPP(*header_packet),
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
return(0);
}
if(htons(ppd->header_ip->tot_len) + ppd->header_ip_offset > HPH(*header_packet)->len) {
pcapProcessEvalError(bad_ip_length, *HPH(*header_packet), HPP(*header_packet),
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
return(0);
}
} else {
pcapProcessEvalError(bad_datalink, *HPH(*header_packet), HPP(*header_packet),
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
return(0);
}
} else {
pcap_header_plus2 = (pcap_pkthdr_plus2*)block_store->get_header(block_store_index);
packet = block_store->get_packet(block_store_index);
if(pcap_header_plus2->detect_headers & 0x01) {
ppd->header_ip_offset = pcap_header_plus2->header_ip_first_offset;
ppd->protocol = pcap_header_plus2->eth_protocol;
ppd->header_ip = (iphdr2*)(packet + ppd->header_ip_offset);
} else if(parseEtherHeader(pcapLinklayerHeaderType, packet,
ppd->header_sll, ppd->header_eth, ppd->header_ip_offset, ppd->protocol)) {
pcap_header_plus2->detect_headers |= 0x01;
pcap_header_plus2->header_ip_first_offset = ppd->header_ip_offset;
pcap_header_plus2->eth_protocol = ppd->protocol;
if(ppd->protocol != ETHERTYPE_IP) {
if(sverb.tcpreplay) {
if(ppd->protocol == 0) {
ppd->header_ip_offset += 2;
ppd->protocol = ETHERTYPE_IP;
} else {
pcap_header_plus2->ignore = true;
return(0);
}
} else {
pcapProcessEvalError(bad_eth_protocol, pcap_header_plus2, packet,
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
pcap_header_plus2->ignore = true;
return(0);
}
}
ppd->header_ip = (iphdr2*)(packet + ppd->header_ip_offset);
if(ppd->header_ip->version != 4) {
pcapProcessEvalError(bad_ip_version, pcap_header_plus2, packet,
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
pcap_header_plus2->ignore = true;
return(0);
}
if(htons(ppd->header_ip->tot_len) + ppd->header_ip_offset > pcap_header_plus2->get_len()) {
pcapProcessEvalError(bad_ip_length, pcap_header_plus2, packet,
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
pcap_header_plus2->ignore = true;
return(0);
}
} else {
pcapProcessEvalError(bad_datalink, pcap_header_plus2, packet,
ppd, pcapLinklayerHeaderType, pcapDumpHandle, interfaceName);
pcap_header_plus2->ignore = true;
return(0);
}
}
if(ppf & ppf_defrag) {
//if UDP defrag is enabled process only UDP packets and only SIP packets
if(opt_udpfrag) {
int foffset = ntohs(ppd->header_ip->frag_off);
if ((foffset & IP_MF) || ((foffset & IP_OFFSET) > 0)) {
if(htons(ppd->header_ip->tot_len) + ppd->header_ip_offset > HPH(*header_packet)->caplen) {
if(interfaceName) {
extern BogusDumper *bogusDumper;
static u_long lastTimeLogErrBadIpHeader = 0;
if(bogusDumper) {
bogusDumper->dump(HPH(*header_packet), HPP(*header_packet), pcapLinklayerHeaderType, interfaceName);
}
u_long actTime = getTimeMS(HPH(*header_packet));
if(actTime - 1000 > lastTimeLogErrBadIpHeader) {
syslog(LOG_ERR, "BAD FRAGMENTED HEADER_IP: %s: bogus ip header length %i, caplen %i", interfaceName, htons(ppd->header_ip->tot_len), HPH(*header_packet)->caplen);
lastTimeLogErrBadIpHeader = actTime;
}
}
//cout << "pcapProcess exit 001" << endl;
return(0);
}
// packet is fragmented
if(handle_defrag(ppd->header_ip, header_packet, &ppd->ipfrag_data, pushToStack_queue_index) > 0) {
// packets are reassembled
ppd->header_ip = (iphdr2*)(HPP(*header_packet) + ppd->header_ip_offset);
if(sverb.defrag) {
defrag_counter++;
cout << "*** DEFRAG 1 " << defrag_counter << endl;
}
} else {
//cout << "pcapProcess exit 002" << endl;
return(0);
}
}
}
}
bool nextPass;
do {
nextPass = false;
u_int first_header_ip_offset = ppd->header_ip_offset;
if(ppd->header_ip->protocol == IPPROTO_IPIP) {
// ip in ip protocol
ppd->header_ip = (iphdr2*)((char*)ppd->header_ip + sizeof(iphdr2));
ppd->header_ip_offset += sizeof(iphdr2);
} else if(ppd->header_ip->protocol == IPPROTO_GRE) {
// gre protocol
iphdr2 *header_ip = convertHeaderIP_GRE(ppd->header_ip);
if(header_ip) {
ppd->header_ip = header_ip;
ppd->header_ip_offset = (u_char*)header_ip - (header_packet ? HPP(*header_packet) : packet);
nextPass = true;
} else {
if(ppf & ppf_returnZeroInCheckData) {
//cout << "pcapProcess exit 004" << endl;
if(pcap_header_plus2) {
pcap_header_plus2->ignore = true;
}
return(0);
}
}
} else {
break;
}
if(ppf & ppf_defrag) {
//if UDP defrag is enabled process only UDP packets and only SIP packets
if(opt_udpfrag && ppd->header_ip->protocol == IPPROTO_UDP) {
int foffset = ntohs(ppd->header_ip->frag_off);
if ((foffset & IP_MF) || ((foffset & IP_OFFSET) > 0)) {
// packet is fragmented
if(handle_defrag(ppd->header_ip, header_packet, &ppd->ipfrag_data, pushToStack_queue_index) > 0) {
// packets are reassembled
iphdr2 *first_header_ip = (iphdr2*)(HPP(*header_packet) + first_header_ip_offset);
// turn off frag flag in the first IP header
first_header_ip->frag_off = 0;
// turn off frag flag in the second IP header
ppd->header_ip = (iphdr2*)(HPP(*header_packet) + ppd->header_ip_offset);
ppd->header_ip->frag_off = 0;
// update lenght of the first ip header to the len of the second IP header since it can be changed due to reassemble
first_header_ip->tot_len = htons(ntohs(ppd->header_ip->tot_len) + (ppd->header_ip_offset - first_header_ip_offset));
if(sverb.defrag) {
defrag_counter++;
cout << "*** DEFRAG 2 " << defrag_counter << endl;
}
} else {
//cout << "pcapProcess exit 003" << endl;
return(0);
}
}
}
}
} while(nextPass);
if(header_packet) {
(*header_packet)->header_ip_offset = ppd->header_ip_offset;
} else {
pcap_header_plus2->header_ip_offset = ppd->header_ip_offset;
}
if(ppf & ppf_defrag) {
// if IP defrag is enabled, run each 10 seconds cleaning
if(opt_udpfrag && (ppd->ipfrag_lastprune + 10) < HPH(*header_packet)->ts.tv_sec) {
ipfrag_prune(HPH(*header_packet)->ts.tv_sec, false, &ppd->ipfrag_data, pushToStack_queue_index, -1);
ppd->ipfrag_lastprune = HPH(*header_packet)->ts.tv_sec;
//TODO it would be good to still pass fragmented packets even it does not contain the last semant, the ipgrad_prune just wipes all unfinished frags
}
}
if(!(ppf & ppf_defragInPQout)) {
u_int32_t caplen;
if(header_packet) {
caplen = HPH(*header_packet)->caplen;
packet = HPP(*header_packet);
} else {
caplen = pcap_header_plus2->get_caplen();
}
ppd->header_udp = &ppd->header_udp_tmp;
if (ppd->header_ip->protocol == IPPROTO_UDP) {
// prepare packet pointers
ppd->header_udp = (udphdr2*) ((char*) ppd->header_ip + sizeof(*ppd->header_ip));
ppd->data = (char*) ppd->header_udp + sizeof(*ppd->header_udp);
ppd->datalen = (int)(caplen - ((u_char*)ppd->data - packet));
ppd->traillen = (int)(caplen - ((u_char*)ppd->header_ip - packet)) - ntohs(ppd->header_ip->tot_len);
ppd->istcp = 0;
} else if (ppd->header_ip->protocol == IPPROTO_TCP) {
ppd->istcp = 1;
// prepare packet pointers
ppd->header_tcp = (tcphdr2*) ((char*) ppd->header_ip + sizeof(*ppd->header_ip));
ppd->data = (char*) ppd->header_tcp + (ppd->header_tcp->doff * 4);
ppd->datalen = (int)(caplen - ((u_char*)ppd->data - packet));
if (!(sipportmatrix[htons(ppd->header_tcp->source)] || sipportmatrix[htons(ppd->header_tcp->dest)]) &&
!(opt_enable_http && (httpportmatrix[htons(ppd->header_tcp->source)] || httpportmatrix[htons(ppd->header_tcp->dest)]) &&
(tcpReassemblyHttp->check_ip(htonl(ppd->header_ip->saddr)) || tcpReassemblyHttp->check_ip(htonl(ppd->header_ip->daddr)))) &&
!(opt_enable_webrtc && (webrtcportmatrix[htons(ppd->header_tcp->source)] || webrtcportmatrix[htons(ppd->header_tcp->dest)]) &&
(tcpReassemblyWebrtc->check_ip(htonl(ppd->header_ip->saddr)) || tcpReassemblyWebrtc->check_ip(htonl(ppd->header_ip->daddr)))) &&
!(opt_enable_ssl &&
(isSslIpPort(htonl(ppd->header_ip->saddr), htons(ppd->header_tcp->source)) ||
isSslIpPort(htonl(ppd->header_ip->daddr), htons(ppd->header_tcp->dest)))) &&
!(opt_skinny && (htons(ppd->header_tcp->source) == 2000 || htons(ppd->header_tcp->dest) == 2000))) {
// not interested in TCP packet other than SIP port
if(!opt_ipaccount && !DEBUG_ALL_PACKETS && (ppf & ppf_returnZeroInCheckData)) {
//cout << "pcapProcess exit 005" << endl;
if(pcap_header_plus2) {
pcap_header_plus2->ignore = true;
}
return(0);
}
}
ppd->header_udp->source = ppd->header_tcp->source;
ppd->header_udp->dest = ppd->header_tcp->dest;
} else {
//packet is not UDP and is not TCP, we are not interested, go to the next packet (but if ipaccount is enabled, do not skip IP
ppd->datalen = 0;
if(!opt_ipaccount && !DEBUG_ALL_PACKETS && (ppf & ppf_returnZeroInCheckData)) {
//cout << "pcapProcess exit 006 / protocol: " << (int)ppd->header_ip->protocol << endl;
if(pcap_header_plus2) {
pcap_header_plus2->ignore = true;
}
return(0);
}
}
if(ppd->datalen < 0 && (ppf & ppf_returnZeroInCheckData)) {
//cout << "pcapProcess exit 007" << endl;
if(pcap_header_plus2) {
pcap_header_plus2->ignore = true;
}
return(0);
}
} else {
ppd->data = NULL;
ppd->datalen = 0;
}
if((ppf & ppf_calcMD5) || (ppf & ppf_dedup)) {
// check for duplicate packets (md5 is expensive operation - enable only if you really need it
if(opt_dup_check &&
ppd->prevmd5s != NULL &&
((ppf & ppf_defragInPQout) ||
(ppd->datalen > 0 && (opt_dup_check_ipheader || ppd->traillen < ppd->datalen))) &&
!(ppd->istcp && opt_enable_http && (httpportmatrix[htons(ppd->header_tcp->source)] || httpportmatrix[htons(ppd->header_tcp->dest)])) &&
!(ppd->istcp && opt_enable_webrtc && (webrtcportmatrix[htons(ppd->header_tcp->source)] || webrtcportmatrix[htons(ppd->header_tcp->dest)])) &&
!(ppd->istcp && opt_enable_ssl && (isSslIpPort(htonl(ppd->header_ip->saddr), htons(ppd->header_tcp->source)) || isSslIpPort(htonl(ppd->header_ip->daddr), htons(ppd->header_tcp->dest))))) {
uint16_t *_md5 = header_packet ? (*header_packet)->md5 : pcap_header_plus2->md5;
if(ppf & ppf_calcMD5) {
MD5_Init(&ppd->ctx);
if(ppf & ppf_defragInPQout) {
u_int32_t caplen = header_packet ? HPH(*header_packet)->caplen : pcap_header_plus2->get_caplen();
MD5_Update(&ppd->ctx, ppd->header_ip, MIN(caplen - ppd->header_ip_offset, ntohs(ppd->header_ip->tot_len)));
} else if(opt_dup_check_ipheader) {
MD5_Update(&ppd->ctx, ppd->header_ip, MIN(ppd->datalen + (ppd->data - (char*)ppd->header_ip), ntohs(ppd->header_ip->tot_len)));
} else {
// check duplicates based only on data (without ip header and without UDP/TCP header). Duplicate packets
// will be matched regardless on IP
MD5_Update(&ppd->ctx, ppd->data, MAX(0, (unsigned long)ppd->datalen - ppd->traillen));
}
MD5_Final((unsigned char*)_md5, &ppd->ctx);
}
if((ppf & ppf_dedup) && _md5[0]) {
if(memcmp(_md5, ppd->prevmd5s + (_md5[0] * MD5_DIGEST_LENGTH), MD5_DIGEST_LENGTH) == 0) {
//printf("dropping duplicate md5[%s]\n", md5);
duplicate_counter++;
if(sverb.dedup) {
cout << "*** DEDUP " << duplicate_counter << endl;
}
if(pcap_header_plus2) {
pcap_header_plus2->ignore = true;
}
return(0);
}
memcpy(ppd->prevmd5s + (_md5[0] * MD5_DIGEST_LENGTH), _md5, MD5_DIGEST_LENGTH);
}
}
}
if(ppf & ppf_dump) {
if(pcapDumpHandle) {
if(header_packet) {
pcap_dump((u_char*)pcapDumpHandle, HPH(*header_packet), HPP(*header_packet));
} else {
pcap_pkthdr header = pcap_header_plus2->getStdHeader();
pcap_dump((u_char*)pcapDumpHandle, &header, packet);
}
}
}
return(1);
}
