#ifndef __NETWPROTOS_H__ #define __NETWPROTOS_H__ #include #include #include "vectorutils.h" #include "stringutils.h" #include "errno.h" #include #define dbgsum(...) // g++ networkprotocols.cpp -I /opt/local/include/ -I ../common -D_UNIX -I ../include/win32 /* * rawdata <-> Qin,out <-> pppprotocol * c021 <-> lcpq <-> LCPProtocol * c023 <-> papq <-> PAPProtocol * 8021 <-> ipcpq <-> IPCPProtocol * 21 <-> ipq <-> IPProtocol * 17 <-> udpq <-> UDPProtocol * 6 <-> tcpq <-> TCPProtocol * 990 <-> rapiq <-> RapiProtocol * */ #include "err/posix.h" class capreader { typedef boost::shared_ptr FILEptr; FILEptr _f; struct capheader { uint32_t magic; uint16_t versionmaj; uint16_t versionmin; uint32_t thiszone; uint32_t sigfigs; uint32_t snaplen; uint32_t linktype; }; struct pkthdr { uint32_t sec; uint32_t usec; uint32_t caplen; uint32_t wirelen; }; capheader _hdr; public: // 0 : loopback device ? // 1 : ethernet: 6 + 6 addr then 2 byte type, then payload //50 : ff03 ppp frames ( or ff01+ff02 type frames ) // 9 : ff03 ppp frames or c021... ppp frames int type() const { return _hdr.linktype; } capreader(const std::string& filename) { _f= FILEptr(fopen(filename.c_str(), "rb"), fclose); if (!_f) throw posixerror("fopen"); if (1!=fread(&_hdr, sizeof(_hdr), 1, _f.get())) throw posixerror("fread(hdr)"); if (_hdr.magic!=0xa1b2c3d4) throw "cap file magic"; printf("%s: %d.%d zone%x sig%x snap=%d, lt=%d\n", filename.c_str(), _hdr.versionmaj, _hdr.versionmin, _hdr.thiszone, _hdr.sigfigs, _hdr.snaplen, _hdr.linktype); } ByteVector next() { pkthdr ph; if (1!=fread(&ph, sizeof(ph), 1, _f.get())) throw posixerror("fread(pkthdr)"); ByteVector pkt(ph.caplen); if (1!=fread(&pkt[0], ph.caplen, 1, _f.get())) throw posixerror("fread(pktdata)"); return pkt; } bool eof() { return feof(_f.get()); } }; // .. i think i should add a link from ctx -> layer // so 'lowersend' does not need a fixed 'lower', but can send through ctx only. struct context { typedef boost::shared_ptr ptr; virtual int id() const { return -1; } virtual std::string asstring() const { return "?"; } std::string rstr() const { return (lower?lower->rstr()+",":"")+asstring(); } context() { } explicit context(ptr lower) : lower(lower) { } ptr lower; // example: ipcontext contains ptr to pppcontext }; // difficult to make const correct: i need both a version // with const and non-const _begin+_end struct pktdata { typedef uint8_t* iterator; pktdata() : _begin(0), _end(0) { } pktdata(ByteVector& bv) : _begin(&bv.front()), _end(&bv.back()+1) { } pktdata(uint8_t* a, uint8_t* b) : _begin(a), _end(b) { } pktdata(uint8_t* a, size_t n) : _begin(a), _end(a+n) { } uint8_t* _begin; uint8_t* _end; uint8_t* begin() const { return _begin; } uint8_t* end() const { return _end; } // IMPORTANT NOTE: my reverse iterators don't handle '++' as they should. uint8_t* rbegin() const { return _end-1; } uint8_t* rend() const { return _begin-1; } size_t size() const { return _end-_begin; } uint8_t* ptr() const { return _begin; } std::string hexdump() const { return ::hexdump(ptr(), size()); } void resize(size_t n) { _end= _begin+n; } void moveend(int n) { _end+=n; } void movebegin(int n) { _begin+=n; } uint8_t& front() const { return _begin[0]; } uint8_t& back() const { return _end[-1]; } uint8_t& operator[](size_t i) const { return _begin[i]; } // get with offset gets either from start or end ( off<0 ) uint8_t get8(int off) const { return off>=0 ? _begin[off] : _end[off]; } uint16_t getnet16(int off) const { return get8(off)*256+get8(off+1); } uint32_t getnet32(int off) const { return getnet16(off)*256*256+getnet16(off+2); } uint16_t get16(int off) const { return get8(off+1)*256+get8(off); } uint32_t get32(int off) const { return get16(off+2)*256*256+get16(off); } // get without param gets and moves begin ptr uint8_t get8() { return *_begin++; } uint16_t getnet16() { uint8_t c= get8(); return c*256+get8(); } uint32_t getnet32() { uint16_t c= getnet16(); return c*256*256+getnet16(); } uint16_t get16() { uint8_t c= get8(); return c+256*get8(); } uint32_t get32() { uint16_t c= get16(); return c+256*256*get16(); } // pull without param moves and gets from end ptr uint8_t pull8() { return *--_end; } uint16_t pullnet16() { uint8_t c= pull8(); return c+256*pull8(); } uint32_t pullnet32() { uint16_t c= pullnet16(); return c+256*256*pullnet16(); } uint16_t pull16() { uint8_t c= pull8(); return c*256+pull8(); } uint32_t pull32() { uint16_t c= pull16(); return c*256*256+pull16(); } // set at specified offset void set8(int off, uint8_t c) const { if (off<0) _end[off] = c; else _begin[off]= c; } void setnet16(int off, uint16_t c) const { set8(off, c>>8); set8(off+1, c&0xff); } void setnet32(int off, uint32_t c) const { setnet16(off, c>>16); setnet16(off+2, c&0xffff); } void set16(int off, uint16_t c) const { set8(off, c&0xff); set8(off+1, c>>8); } void set32(int off, uint32_t c) const { set16(off, c&0xffff); set16(off+2, c>>16); } // add to end of buffer void put8(uint8_t c) { *_end++ = c; } void putnet16(uint16_t c) { put8(c>>8); put8(c&0xff); } void putnet32(uint32_t c) { putnet16(c>>16); putnet16(c&0xffff); } void put16(uint16_t c) { put8(c&0xff); put8(c>>8); } void put32(uint32_t c) { put16(c&0xffff); put16(c>>16); } void put(const ByteVector&v) { std::copy(v.begin(), v.end(), _end); _end += v.size(); } void put(const pktdata& pkt) { std::copy(pkt.begin(), pkt.end(), _end); _end += pkt.size(); } // add to front of buffer void push8(uint8_t c) { *--_begin = c; } void pushnet16(uint16_t c) { push8(c&0xff); push8(c>>8); } void pushnet32(uint32_t c) { pushnet16(c&0xffff); pushnet16(c>>16); } void push16(uint16_t c) { push8(c>>8); push8(c&0xff); } void push32(uint32_t c) { push16(c>>16); push16(c&0xffff); } void push(const ByteVector&v) { _begin -= v.size(); std::copy(v.begin(), v.end(), _begin); } }; class protocollayer : public boost::enable_shared_from_this { public: typedef boost::shared_ptr ptr; typedef std::map protocolmap_t; // note: the pkt must have sufficient headroom virtual void send(context::ptr ctx, pktdata& pkt)=0; virtual void recv(context::ptr ctx, pktdata& pkt)=0; void addprotocol(protocollayer::ptr l, context::ptr ctx=context::ptr()) { // todo: think of a way to add multiple lower layer protocols ( like ethernet->ip and ppp->ip ) l->setlower(shared_from_this(), ctx); _m[ctx ? ctx->id() : -1]= l; } void setlower(protocollayer::ptr l, context::ptr ctx) { _lower= l; _ctx= ctx; } void lowersend(context::ptr ctx, pktdata& data) { printf("send [%s] : %s\n", ctx?ctx->rstr().c_str():"?", data.hexdump().c_str()); // either use the context in ctx->lower, or the default from _ctx _lower->send((ctx && ctx->lower) ? ctx->lower : _ctx, data); } protocollayer::ptr find(int id=-1) const { protocolmap_t::const_iterator i= _m.find(id); if (i==_m.end()) { printf("could not find 0x%x in %s\n", id, _ctx ? _ctx->rstr().c_str():"?"); return protocollayer::ptr(); } //printf("forwarding 0x%x to %s\n", id, _ctx ? _ctx->rstr().c_str():"?"); return (*i).second; } private: protocollayer::ptr _lower; context::ptr _ctx; protocolmap_t _m; }; class network : public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new network()); } virtual void send(context::ptr ctx, pktdata& data) { printf("send: %s\n", data.hexdump().c_str()); } virtual void recv(context::ptr ctx, pktdata&data) { printf("net: recv: %s\n", data.hexdump().c_str()); protocollayer::ptr up= find(); up->recv(context::ptr(), data); } }; // http://www.faqs.org/rfcs/rfc1662.html class pppflagescape : public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new pppflagescape()); } virtual void send(context::ptr ctx, pktdata& data) { size_t count=2; for (pktdata::iterator i= data.begin() ; i!=data.end() ; i++) if (needsescape(*i)) count++; printf("found %zd 2be esc in %p-%p\n", count, data.begin(), data.end()); pktdata::iterator j= data.begin()+count; *j-- = 0x7e; // flag printf("j-- = %p, rb=%p, re=%p\n", j, data.rbegin(), data.rend()); for (pktdata::iterator i= data.rbegin() ; i!=data.rend() ; i--) { if (needsescape(*i)) { *j--= (*i) ^ 0x20; *j--= 0x7d; } else { *j--= *i; } } *j-- = 0x7e; // flag lowersend(ctx, data); } // todo: handle incomplete input - cache stream data virtual void recv(context::ptr ctx, pktdata &data) { printf("pppesc: %s\n", data.hexdump().c_str()); if (data.size()<=2 || data.front()!=0x7e || data.back()!=0x7e) { printf("invalid 7e..7e frame\n"); return; } data.movebegin(1); data.moveend(-1); pktdata::iterator j= data.begin(); for (pktdata::iterator i= data.begin() ; i!=data.end() ; i++) { if ((*i)==0x7d) { i++; if (i==data.end()) { printf("ppp 7d escape at end of frame\n"); return; } *j++ = (*i)^0x20; } else { *j++ = *i; } } data.moveend(j-data.end()); protocollayer::ptr up= find(); up->recv(context::ptr(), data); } bool needsescape(uint8_t b) { return (b==0x7d || b==0x7e || b==0x13 || b==0x11 || b==0x03); } }; class fcscalc { std::vector _tab16; std::vector _tab32; public: fcscalc() { calctab(_tab16, (uint16_t)0x8408); calctab(_tab32, (uint32_t)0x04c11db7); } template void calctab(std::vector& tab, T poly) { for (unsigned b=0 ; b<256 ; b++) { T v= b; for (unsigned i=0 ; i<8 ; i++) v = (v>>1) ^ ((v&1) ? poly : 0 ); tab.push_back(v); } } template T calcfcs(pktdata& v, const std::vector& tab, T fcs) { for (pktdata::iterator i= v.begin() ; i!=v.end() ; i++) { fcs = (fcs>>8) ^ tab[(fcs^(*i))&0xff]; } return fcs; } uint16_t calc16(pktdata& v, uint16_t fcs=0xffff) { return calcfcs(v, _tab16, fcs); } uint32_t calc32(pktdata& v, uint32_t fcs=0xffffffff) { return calcfcs(v, _tab32, fcs); } bool check16(uint16_t fcs) { return fcs==0xf0b8; } bool check32(uint32_t fcs) { return fcs==0xdebb20e3; } }; struct lcpconf { public: // '+' : 0 or more unused bytes enum LcpCodes { ConfReq=1, // 1 Configure-Request | options ConfAck, // 2 Configure-Ack | options ConfNak, // 3 Configure-Nak | options ConfRej, // 4 Configure-Reject | options TermReq, // 5 Terminate-Request | + TermAck, // 6 Terminate-Ack | + CodeRej, // 7 Code-Reject | rejected lcp packet ProtRej, // 8 Protocol-Reject | rejected protocol + packet EchoReq, // 9 Echo-Request | magicnr+ EchoRep, // 10 Echo-Reply | magicnr+ DiscReq, // 11 Discard-Request | magicnr+ }; enum LcpConfTypes { MRU=1, // 1 Maximum-Receive-Unit | default=1500, 2 octet value AsyCtlMap, // 2 Async-Control-Character-Map | 32bits AuthProto, // 3 Authentication-Protocol | c023(pap) or c223(chap) + data QualProto, // 4 Quality-Protocol | c025(lqr) MagicNum, // 5 Magic-Number | 32bit nr unused6, ProtoComp, // 7 Protocol-Field-Compression | - AdrCtlComp, // 8 Address-and-Control-Field-Compression }; struct lcpoption { lcpoption(uint8_t code, uint8_t type) : bv(256), code(code), type(type), data(bv) { data.resize(0); } lcpoption(uint8_t code, uint8_t type, uint32_t u32) : bv(256), code(code), type(type), data(bv) { data.resize(0); data.put32(u32); } lcpoption(uint8_t code, uint8_t type, uint16_t u16) : bv(256), code(code), type(type), data(bv) { data.resize(0); data.put16(u16); } lcpoption(uint8_t code, uint8_t type, const pktdata& pkt) : bv(256), code(code), type(type), data(bv) { data.resize(0); data.put(pkt); } ByteVector bv; uint8_t code; uint8_t type; pktdata data; }; lcpconf() : adccomp(false), mru(1600), magic(0), protocomp(false) { } void handlemru(uint8_t code,uint16_t mruval) { printf("requested mru: %04x\n", mruval); if (code==ConfReq) { mru= mruval; _optreplies.push_back(lcpoption(ConfAck, MRU, mru)); } } void handleauth(uint8_t code,uint16_t authproto) { printf("requested auth protocol: %04x\n", authproto); _optreplies.push_back(lcpoption(ConfNak, AuthProto, authproto)); } void handlequal(uint8_t code,uint16_t qualproto) { printf("requested quality protocol: %04x\n", qualproto); _optreplies.push_back(lcpoption(ConfNak, QualProto, qualproto)); } void handlemagic(uint8_t code,uint32_t mg) { printf("magic: %08x\n", mg); if (code==ConfReq) { magic= mg; _optreplies.push_back(lcpoption(ConfAck, MagicNum, magic)); } } void handleprotocomp(uint8_t code) { printf("protocol compression\n"); if (code==ConfReq) { protocomp= true; _optreplies.push_back(lcpoption(ConfAck, ProtoComp)); } else if (code==ConfAck) { protocomp= true; } } void handleadrctlcomp(uint8_t code) { printf("addr+ctl compression\n"); if (code==ConfReq) { adccomp= true; _optreplies.push_back(lcpoption(ConfAck, AdrCtlComp)); } else if (code==ConfAck) { adccomp= true; } } void handleunknown(uint8_t code, uint8_t type, const pktdata& pkt) { printf("unknown type: %02x\n", type); _optreplies.push_back(lcpoption(ConfRej, type, pkt)); // what happens if i receive a rej/nak for a option i did not req } ByteVector collect(int code) { ByteVector bv; for (unsigned i=0 ; i<_optreplies.size() ; i++) { if (_optreplies[i].code==code) { bv.push_back(_optreplies[i].type); bv.push_back(_optreplies[i].data.size()+2); if (_optreplies[i].data.size()) BV_AppendBytes(bv, _optreplies[i].data.ptr(), _optreplies[i].data.size()); } } return bv; } uint8_t newlcpid() { return 123; } // todo - make sure this increments with each request bool adccomp; uint16_t mru; uint32_t magic; bool protocomp; std::vector _optreplies; }; lcpconf _conf; // global config object - todo: put it somewhere it belongs // http://www.faqs.org/rfcs/rfc1662.html class ppphdlcframing : public protocollayer { fcscalc _fcs; int _fcssize; public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppphdlcframing()); } ppphdlcframing() : _fcssize(16) { } virtual void send(context::ptr ctx, pktdata& data) { if (!_conf.adccomp) { data.push8(0x03); data.push8(0xff); } if (_fcssize==16) data.put16(~_fcs.calc16(data)); else if (_fcssize==32) data.put32(~_fcs.calc32(data)); else throw "unsupported fcs"; lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata&data) { printf("ppphdlc: %s\n", data.hexdump().c_str()); if (data.size()<4) { printf("hdlc frame too short\n"); return; } pktdata::iterator i= data.begin(); if (*(i)==0xff && *(i+1)==0x03) // skip address i+=2; // todo: handle fcs32 pktdata bv(i, data.end()); uint16_t fcs= _fcs.calc16(data); if (!_fcs.check16(fcs)) { pktdata data2(data.begin(), data.end()-2); printf("hdlc frame fcs mismatch : %04x .. %04x\n", fcs, _fcs.calc16(data2)); return; } bv.resize(bv.size()-2); protocollayer::ptr up= find(); up->recv(context::ptr(), bv); } }; struct pppcontext : context { typedef boost::shared_ptr ptr; pppcontext(int proto) : proto(proto) { } static ptr make(int proto) { return ptr(new pppcontext(proto)); } pppcontext(context::ptr ctx) : context(ctx), proto(0) { } static ptr make(context::ptr ctx) { return ptr(new pppcontext(ctx)); } int proto; virtual int id() const { return proto; } virtual std::string asstring() const { return stringformat("ppp[%04x]", proto); } }; struct lcpcontext : context { typedef boost::shared_ptr ptr; lcpcontext(uint8_t code, uint8_t ident) : code(code), ident(ident) { } static ptr make(uint8_t code, uint8_t ident) { return ptr(new lcpcontext(code,ident)); } lcpcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new lcpcontext(ctx)); } struct lcpreply { lcpreply(uint8_t code, uint8_t id, const pktdata& pkt) : bv(256), code(code), id(id), data(bv) { data.movebegin(64); data.resize(0); data.put(pkt); printf("lcpreply: %p -64 - %p\n", data.begin(), data.end()); } ByteVector bv; uint8_t code; uint8_t id; pktdata data; }; std::vector _lcpreplies; uint8_t code; uint8_t ident; virtual int id() const { return ident; } virtual std::string asstring() const { return stringformat("lcp[%02x:%02x]", code,ident); } // options: // type | length | .....data ..... // length = 2+sizeof(data) void parseoptions(pktdata& data) { while (data.size()>=2) { uint8_t type= data.get8(); uint8_t len= data.get8(); switch(type) { case lcpconf::MRU: if (data.size()>=len && len>=2) { _conf.handlemru(code,data.getnet16(0)); } break; // case lcpconf::AsyCtlMap: // break; case lcpconf::AuthProto: if (data.size()>=len && len>=2) { _conf.handleauth(code,data.getnet16(0)); } break; case lcpconf::QualProto: if (data.size()>=len && len>=2) { _conf.handlequal(code,data.getnet16(0)); } break; case lcpconf::MagicNum: if (data.size()>=len && len>=4) { _conf.handlemagic(code,data.getnet32(0)); } break; case lcpconf::ProtoComp: _conf.handleprotocomp(code); break; case lcpconf::AdrCtlComp: _conf.handleadrctlcomp(code); break; default: _conf.handleunknown(code, type, pktdata(data.begin(), data.begin()+len)); } data.movebegin(len-2); } } void parsemagic(pktdata& data) { if (data.size()<4) { printf("invalid lcp magic\n"); return; } uint32_t magic= data.getnet32(); if (magic!=_conf.magic) printf("lcp ping: magic = %08x, conf=%08x\n", magic, _conf.magic); } void parsenodata(pktdata& data) { // ignore } void parserejected(pktdata& data) { printf("rejected %s : %s\n", code==lcpconf::CodeRej?"LCP":"PPP", data.hexdump().c_str()); } void parsedata(pktdata& data) { if (code>=lcpconf::ConfReq && code<=lcpconf::ConfRej) { parseoptions(data); // todo: process _conf.replies and create the right lcp replies // collect ack // collect nak // collect rej // collect req int codes[4]= { lcpconf::ConfAck, lcpconf::ConfNak, lcpconf::ConfRej, lcpconf::ConfReq }; for (unsigned i=0 ; i<4 ; i++) { ByteVector bv(_conf.collect(codes[i])); pktdata acks(bv); if (acks.size()) { _lcpreplies.push_back(lcpreply(codes[i], ident, acks)); printf("todo:lcp code %d\n", codes[i]); } } } else if (code>=lcpconf::EchoReq && code<=lcpconf::DiscReq) { parsemagic(data); if (code==lcpconf::EchoReq) { _lcpreplies.push_back(lcpreply(lcpconf::EchoRep,ident, pktdata())); printf("todo:lcp echoreply\n"); } } else if (code==lcpconf::TermReq || code==lcpconf::TermAck) { parsenodata(data); if (code==lcpconf::TermReq) { _lcpreplies.push_back(lcpreply(lcpconf::TermAck,ident, pktdata())); printf("todo:lcp termack\n"); } } else if (code==lcpconf::ProtRej || code==lcpconf::CodeRej) { parserejected(data); } else { // todo: respond with lcpconf::CodeRej packet _lcpreplies.push_back(lcpreply(lcpconf::CodeRej, _conf.newlcpid(), data)); printf("todo:lcp coderej\n"); } } }; class ppp_lcp; // http://www.faqs.org/rfcs/rfc1661.html class pppprotocol : public protocollayer { // protocol ranges: // 0..3 xxx : network layer protocols // 4..7 xxx : low volume traffic // 8..b xxx : network control protocols // c..f xxx : link layer control protocols bool _protocolfieldcompression; public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new pppprotocol()); } pppprotocol() : _protocolfieldcompression(false) { } virtual void send(context::ptr ctx, pktdata& data) { pppcontext::ptr ppp= boost::dynamic_pointer_cast(ctx); if ((ppp->proto&1)==0 || (ppp->proto&0x100)!=0) { printf("pppproto=%x\n", ppp->proto); throw "ppp:invalid protocol nr\n"; } if (!_protocolfieldcompression && ppp->proto>=0x100) { data.pushnet16(ppp->proto); lowersend(ctx, data); } else { data.push8(ppp->proto); lowersend(ctx, data); } } virtual void recv(context::ptr ctx, pktdata& data) { printf("ppp: %s\n", data.hexdump().c_str()); int i= 0; if (data.size()<2) { printf("invalid ppp frame\n"); return; } pppcontext::ptr ppp= pppcontext::make(ctx); int proto= data.get8(i++); if ((proto&1)==0) proto= (proto<<8) | data.get8(i++); ppp->proto= proto; data.movebegin(i); protocollayer::ptr up= find(proto); if (up) up->recv(ppp, data); else { printf("rejecting ppp frame for proto 0x%x\n", proto); lcpcontext::ptr lcp= lcpcontext::make(ppp); lcp->ident= _conf.newlcpid(); lcp->code= lcpconf::ProtRej; protocollayer::ptr up= find(0xc021); up->send(lcp, data); } } }; // http://www.faqs.org/rfcs/rfc1661.html class ppp_lcp : public protocollayer { // c021 Link Control Protocol public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_lcp()); } virtual void send(context::ptr ctx, pktdata& data) { lcpcontext::ptr lcp= boost::dynamic_pointer_cast(ctx); if (data.size()>=0x10000) throw "lcp:too much data"; data.pushnet16(data.size()+4); data.push8(lcp->ident); data.push8(lcp->code); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata& data) { printf("lcp: %s\n", data.hexdump().c_str()); if (data.size()<4) { printf("invalid lcp frame\n"); return; } size_t pktsize= data.size(); lcpcontext::ptr lcp= lcpcontext::make(ctx); lcp->code= data.get8(); lcp->ident= data.get8(); size_t len= data.getnet16(); // incl code+ident if (pktsize < len) { printf("invalid lcp: lcplen(%zd)>pktlen(%zd)\n", len, pktsize); return; } if (pktsize > len) { printf("invalid lcp: extra data: %zd bytes\n", pktsize-len); return; } printf("lcp replies=%zd\n", lcp->_lcpreplies.size()); lcp->parsedata(data); printf("sending %zd lcp replies\n", lcp->_lcpreplies.size()); // send lcp replies for (unsigned i=0 ; i_lcpreplies.size() ; i++) { lcpcontext::lcpreply& lcprp= lcp->_lcpreplies[i]; lcpcontext::ptr reply= lcpcontext::make(ctx); reply->code= lcprp.code; reply->ident= lcprp.id; send(reply, lcprp.data); } } }; class ppp_ip6cp : public protocollayer { // 8057 Password Authentication Protocol public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_ip6cp()); } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("ip6cp: %s\n", data.hexdump().c_str()); // todo } }; class ppp_ipcp : public protocollayer { // 8021 IPCP public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_ipcp()); } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("ipcp: %s\n", data.hexdump().c_str()); // todo } }; class ppp_pap : public protocollayer { // c023 Password Authentication Protocol public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_pap()); } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("pap: %s\n", data.hexdump().c_str()); // todo } }; class ppp_lqr : public protocollayer { // c025 Link Quality Report public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_lqr()); } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("lqr: %s\n", data.hexdump().c_str()); // todo } }; class ppp_chap : public protocollayer { // c223 Challenge Handshake Authentication Protocol public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ppp_chap()); } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("chap: %s\n", data.hexdump().c_str()); // todo } }; // the rndis layer as used by activesync in wm6 devices for RNDIS struct rndiscontext : context { typedef boost::shared_ptr ptr; rndiscontext(uint16_t type) : type(type) { } static ptr make(uint16_t type) { return ptr(new rndiscontext(type)); } rndiscontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new rndiscontext(ctx)); } uint16_t type; ByteVector src; ByteVector dst; virtual int id() const { return type; } std::string addrstring(const ByteVector& v) const { if (v.size()==6) return stringformat("%02x:%02x:%02x:%02x:%02x:%02x", v[0], v[1], v[2], v[3], v[4], v[5]); else return "???"; } virtual std::string asstring() const { return stringformat("rndis[%04x : src=%s dst=%s]", type, addrstring(src).c_str(), addrstring(dst).c_str()); } }; class rndisprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new rndisprotocol()); } rndisprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { rndiscontext::ptr rndis= boost::dynamic_pointer_cast(ctx); size_t pldlen= data.size(); size_t pktlen= 0x24+data.size(); if (pktlen&3) { for (unsigned i=0 ; i< (4-(pktlen&3)) ; i++) data.put8(0); pktlen += 4-(pktlen&3); } for (int i=0 ; i<7 ; i++) data.push32(0); data.push32(pldlen); data.push32(0x24); data.push32(pktlen); data.push32(rndis->type); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata& data) { printf("rndis: %s\n", data.hexdump().c_str()); rndiscontext::ptr rndis= rndiscontext::make(ctx); if (data.size()<14) { printf("rndis pkt < 14\n"); return; } rndis->type= data.get32(); /*size_t pktlen=*/ data.get32(); size_t hdrlen= data.get32(); size_t pldlen= data.get32(); data.movebegin(hdrlen-16); if (pldlen>data.size()) { printf("WARN: rndis pkt too small(%zd), pld=%zd\n", data.size(), pldlen); return; } data.resize(pldlen); protocollayer::ptr up= find(rndis->type); if (up) up->recv(rndis, data); else { printf("dropping unsupported rndis type: %d\n", rndis->type); } } }; struct ethcontext : context { typedef boost::shared_ptr ptr; ethcontext(uint16_t type) : type(type) { } static ptr make(uint16_t type) { return ptr(new ethcontext(type)); } ethcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new ethcontext(ctx)); } uint16_t type; ByteVector src; ByteVector dst; virtual int id() const { return type; } std::string addrstring(const ByteVector& v) const { if (v.size()==6) return stringformat("%02x:%02x:%02x:%02x:%02x:%02x", v[0], v[1], v[2], v[3], v[4], v[5]); else return "???"; } virtual std::string asstring() const { return stringformat("eth[%04x : src=%s dst=%s]", type, addrstring(src).c_str(), addrstring(dst).c_str()); } }; class ethprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ethprotocol()); } ethprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { ethcontext::ptr eth= boost::dynamic_pointer_cast(ctx); data.pushnet16(eth->type); data.push(eth->src); data.push(eth->dst); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata& data) { printf("eth: %s\n", data.hexdump().c_str()); ethcontext::ptr eth= ethcontext::make(ctx); if (data.size()<14) { printf("eth pkt < 14\n"); return; } eth->dst= ByteVector(data.begin(), data.begin()+6); eth->src= ByteVector(data.begin()+6, data.begin()+12); data.movebegin(12); eth->type= data.getnet16(); protocollayer::ptr up= find(eth->type); if (up) up->recv(eth, data); else { printf("dropping unsupported eth type: %d\n", eth->type); } } }; class onescomplementsum { uint32_t _sum; public: onescomplementsum() : _sum(0) { dbgsum("sum: "); } template void add(I i, I end) { while (i>16); add(x&0xffff); } void sub(uint32_t x) { dbgsum("-%04x", x); _sum -= x; } uint16_t final() { while (_sum>>16) { _sum = (_sum&0xffff)+(_sum>>16); } dbgsum(" = %04x -> ~=%04x\n", _sum, (uint16_t)~_sum); return ~_sum; } }; struct ipcontext : context { typedef boost::shared_ptr ptr; ipcontext(int proto) : version(4), optionsize(0), tos(0), ttl(40), proto(proto), src(0), dst(0) { } static ptr make(int proto) { return ptr(new ipcontext(proto)); } ipcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new ipcontext(ctx)); } int version; int optionsize; uint8_t tos; uint8_t ttl; uint16_t pktid; int flags; // 0.DF.MF ( DF=dontfrag, MF=morefrag ) int fragofs; // in 8 byte units uint8_t proto; uint32_t src; uint32_t dst; virtual int id() const { return proto; } virtual std::string asstring() const { return stringformat("ip[v%d o%d tos%x ttl%d id%x fl%x of%x proto=%d src=%x dst=%x]", version,optionsize,tos,ttl,pktid,flags,fragofs,proto,src,dst); } void parseoptions(pktdata& data) { // todo handle options: now just skipping data.movebegin(optionsize*4); } void addoptions(pktdata& data) { // todo: add options data.movebegin(-optionsize*4); } }; class ipprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new ipprotocol()); } ipprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { ipcontext::ptr ip= boost::dynamic_pointer_cast(ctx); size_t payloadsize= data.size(); pktdata::iterator hdrend= data.begin(); ip->addoptions(data); data.pushnet32(ip->dst); data.pushnet32(ip->src); data.pushnet16(0); data.push8(ip->proto); data.push8(ip->ttl); data.pushnet16((ip->flags<<13)+ip->fragofs); data.pushnet16(ip->pktid); data.pushnet16((ip->optionsize+5)*4+payloadsize); data.push8(ip->tos); data.push8((ip->version<<4)|(5+ip->optionsize)); onescomplementsum sumcalc; sumcalc.add(data.begin(), hdrend); uint16_t sum=sumcalc.final(); // patch checksum data.setnet16(10, sum); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata&data) { printf("ip: %s\n", data.hexdump().c_str()); ipcontext::ptr ip= ipcontext::make(ctx); size_t pktsize= data.size(); uint8_t hv= data.get8(); ip->version= hv>>4; ip->optionsize= (hv&0xf)-5; if (ip->optionsize<0) { printf("ip headersize < 5\n"); return; } size_t hlen= (hv&15)*4; if (pktsize < hlen) { printf("iphdr > data\n"); return; } ip->tos= data.get8(); onescomplementsum sumcalc; sumcalc.add((hv<<8)|ip->tos); sumcalc.add(data.begin(), data.begin()+hlen-2); uint16_t size= data.getnet16(); if (pktsize < size) { printf("ip pktsize > data\n"); return; } ip->pktid= data.getnet16(); uint16_t flagofs= data.getnet16(); ip->flags= flagofs>>13; ip->fragofs= flagofs&0x1fff; ip->ttl= data.get8(); ip->proto= data.get8(); uint16_t hdrchecksum= data.getnet16(); sumcalc.sub(hdrchecksum); uint16_t calc= sumcalc.final(); if (calc!=hdrchecksum) { printf("iphdr invalid csum: hdr=%04x calc=%04x\n", hdrchecksum, calc); return; } ip->src= data.getnet32(); ip->dst= data.getnet32(); printf("%02x %02x %04x %04x %04x %02x %02x csum=%04x %08x %08x optlen=%d\n", hv, ip->tos, size, ip->pktid, // CCnn flagofs, ip->ttl, ip->proto, hdrchecksum, // nCCn ip->src, ip->dst, ip->optionsize); // NN ip->parseoptions(data); protocollayer::ptr up= find(ip->proto); if (up) up->recv(ip, data); else printf("dropping unsupported ip proto %d\n", ip->proto); } }; struct tcpcontext : context { typedef boost::shared_ptr ptr; tcpcontext(uint16_t dst) : src(0), dst(dst) { } static ptr make(uint16_t dst) { return ptr(new tcpcontext(dst)); } tcpcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new tcpcontext(ctx)); } uint16_t src; uint16_t dst; uint32_t seqnr; uint32_t acknr; int optionsize; uint8_t flags; // urg.ack.psh.rst.syn.fin uint16_t window; uint16_t urgent; virtual int id() const { return dst; } virtual std::string asstring() const { return stringformat("tcp[src=%04x dst=%04x seq=%08x/%08x o=%d f%x w%d u%d]", src, dst, seqnr, acknr, optionsize, flags, window, urgent); } void parseoptions(pktdata& data) { // todo handle options: now just skipping data.movebegin(optionsize*4); } void addoptions(pktdata& data) { // todo: add options data.movebegin(-optionsize*4); } }; class tcpprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new tcpprotocol()); } tcpprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { tcpcontext::ptr tcp= boost::dynamic_pointer_cast(ctx); tcp->addoptions(data); data.pushnet16(tcp->urgent); data.pushnet16(0); data.pushnet16(tcp->window); data.pushnet16(((tcp->optionsize+5)<<12)|tcp->flags); data.pushnet32(tcp->acknr); data.pushnet32(tcp->seqnr); data.pushnet16(tcp->dst); data.pushnet16(tcp->src); ipcontext::ptr ip= boost::dynamic_pointer_cast(ctx->lower); onescomplementsum sumcalc; // pseudo header sumcalc.add32(ip->src); sumcalc.add32(ip->dst); sumcalc.add(ip->proto); sumcalc.add(data.size()); sumcalc.add(data.begin(), data.end()); uint16_t sum=sumcalc.final(); //printf("tcp-out sum [%s] + [%s] = %04x\n", vhexdump(pseudo).c_str(), vhexdump(pkt).c_str(), sum); data.setnet16(16, sum); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata& data) { printf("tcp: %s\n", data.hexdump().c_str()); tcpcontext::ptr tcp= tcpcontext::make(ctx); uint16_t sizeflags; uint16_t checksum; if (data.size()<20) { printf("tcp pkt < 20\n"); return; } size_t pktsize= data.size(); pktdata::iterator tcphdrbegin= data.begin(); tcp->src = data.getnet16(); tcp->dst = data.getnet16(); tcp->seqnr = data.getnet32(); tcp->acknr = data.getnet32(); sizeflags = data.getnet16(); tcp->optionsize= (sizeflags>>12)-5; if (tcp->optionsize<0) { printf("tcp hlen < 5\n"); return; } tcp->flags= sizeflags&0x3f; size_t hlen= (tcp->optionsize+5)*4; if (hlen>pktsize) { printf("tcp hlen > datasize\n"); return; } tcp->window = data.getnet16(); checksum = data.getnet16(); tcp->urgent = data.getnet16(); tcp->parseoptions(data); ipcontext::ptr ip= boost::dynamic_pointer_cast(ctx); onescomplementsum sumcalc; // pseudo header sumcalc.add32(ip->src); sumcalc.add32(ip->dst); sumcalc.add(ip->proto); sumcalc.add(pktsize); sumcalc.add(tcphdrbegin, data.end()); sumcalc.sub(checksum); uint16_t calc=sumcalc.final(); if (calc!=checksum) { printf("tcp: incorrect checksum: hdr=%04x, calc=%04x\n", checksum, calc); return; } //printf("tcp-in sum [%s] + [%s] = %04x\n", vhexdump(pseudo).c_str(), data.hexdump().c_str(), calc); printf("tcp: %04x %04x %08x %08x %04x %04x csum=%04x %04x optlen=%d\n", tcp->src, tcp->dst, // nn tcp->seqnr, tcp->acknr, // NN sizeflags, tcp->window, // nn checksum, tcp->urgent, tcp->optionsize); // nn protocollayer::ptr up= find(tcp->dst); if (up) up->recv(tcp, data); else { printf("dropping unsupported tcp sock: %d\n", tcp->dst); //send(tcp, data); } } }; struct udpcontext : context { typedef boost::shared_ptr ptr; udpcontext(uint16_t src, uint16_t dst) : src(src), dst(dst) { } static ptr make(uint16_t src, uint16_t dst) { return ptr(new udpcontext(src,dst)); } udpcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new udpcontext(ctx)); } uint16_t src; uint16_t dst; virtual int id() const { return dst; } virtual std::string asstring() const { return stringformat("udp[src=%04x dst=%04x]", src, dst); } }; class udpprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new udpprotocol()); } udpprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { udpcontext::ptr udp= boost::dynamic_pointer_cast(ctx); size_t payloadsize= data.size(); data.pushnet16(0); data.pushnet16(8+payloadsize); data.pushnet16(udp->dst); data.pushnet16(udp->src); ipcontext::ptr ip= boost::dynamic_pointer_cast(ctx->lower); onescomplementsum sumcalc; // pseudo hdr sumcalc.add32(ip->src); sumcalc.add32(ip->dst); sumcalc.add(ip->proto); sumcalc.add(8+payloadsize); sumcalc.add(data.begin(), data.end()); uint16_t sum=sumcalc.final(); data.setnet16(6, sum); lowersend(ctx, data); } virtual void recv(context::ptr ctx, pktdata& data) { printf("udp: %s\n", data.hexdump().c_str()); udpcontext::ptr udp= udpcontext::make(ctx); uint16_t size; uint16_t checksum; if (data.size()<8) { printf("udp pkt < 8\n"); return; } pktdata::iterator udphdrbegin= data.begin(); size_t pktsize= data.size(); udp->src = data.getnet16(); udp->dst = data.getnet16(); size = data.getnet16(); checksum = data.getnet16(); if (size>pktsize) { printf("udp size(%x) > data(%zx)\n", size, data.size()); return; } data.resize(size-8); ipcontext::ptr ip= boost::dynamic_pointer_cast(ctx); onescomplementsum sumcalc; // pseudo hdr sumcalc.add32(ip->src); sumcalc.add32(ip->dst); sumcalc.add(ip->proto); sumcalc.add(size); sumcalc.add(udphdrbegin, data.end()); sumcalc.sub(checksum); uint16_t calc=sumcalc.final(); if (calc!=checksum) { printf("udp: incorrect checksum: hdr=%04x, calc=%04x\n", checksum, calc); return; } protocollayer::ptr up= find(udp->dst); if (up) up->recv(udp, data); else { printf("dropping unsupported udp sock: %d\n", udp->dst); //send(udp, data); } } }; struct icmpcontext : context { typedef boost::shared_ptr ptr; icmpcontext() { } static ptr make() { return ptr(new icmpcontext()); } icmpcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new icmpcontext(ctx)); } virtual int id() const { return 0; } virtual std::string asstring() const { return stringformat("icmp"); } }; class icmpprotocol: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new icmpprotocol()); } icmpprotocol() { } virtual void send(context::ptr ctx, pktdata& data) { // todo } virtual void recv(context::ptr ctx, pktdata& data) { printf("icmp: %s\n", data.hexdump().c_str()); // todo } }; // todo: depending on capreader type, build the right protocol stack. // possibly by examining the first few frames struct capcontext : context { typedef boost::shared_ptr ptr; capcontext(int linktype) : linktype(linktype) { } static ptr make(int linktype) { return ptr(new capcontext(linktype)); } capcontext(context::ptr ctx) : context(ctx) { } static ptr make(context::ptr ctx) { return ptr(new capcontext(ctx)); } int linktype; virtual int id() const { return linktype; } virtual std::string asstring() const { return stringformat("cap[type=%04x]", linktype); } }; class capinput: public protocollayer { public: typedef boost::shared_ptr ptr; static ptr make() { return ptr(new capinput()); } capinput() { } virtual void send(context::ptr ctx, pktdata& data) { capcontext::ptr cap= boost::dynamic_pointer_cast(ctx); // .. write to .cap file } virtual void recv(context::ptr ctx, pktdata& data) { printf("cap: %s\n", data.hexdump().c_str()); capcontext::ptr cap= capcontext::make(ctx); protocollayer::ptr up= find(cap->linktype); if (up) up->recv(cap, data); else { printf("dropping unsupported cap linktype: %d\n", cap->linktype); } } }; #endif