int conn_send_msg(struct conn * conn, uint8_t *rawmsg) { int packetlen = cw_get_hdr_msg_total_len(rawmsg); cw_dbg_msg(DBG_MSG_OUT, conn,rawmsg, packetlen,(struct sockaddr*)&conn->addr); /* Zyxel doesn't count msg element length from behind seqnum */ if (conn->capwap_mode == CW_MODE_ZYXEL){ // XXX val-=3; } uint8_t * ptr = rawmsg; int fragoffset = 0; int hlen = cw_get_hdr_hlen(rawmsg)*4; int mtu = conn->mtu; while (packetlen>mtu){ cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F,1); cw_put_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,mtu,(struct sockaddr*)&conn->addr); if (conn->write(conn,ptr,mtu)<0) return -1; // XXX Fragmentation stuff.. ptr +=mtu-hlen; fragoffset+=(mtu-hlen)/8; packetlen-=mtu-hlen; } if (fragoffset) cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F | CW_FLAG_HDR_L,1); else cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F,0); cw_put_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,packetlen,(struct sockaddr*)&conn->addr); return conn->write(conn,ptr,packetlen-0); }
int conn_send_data_msg(struct conn * conn, uint8_t *rawmsg,int len) { int packetlen = len; int fragoffset; int hlen; uint8_t * ptr; int mtu; cw_dbg_msg(DBG_MSG_OUT, conn,rawmsg, packetlen,(struct sockaddr*)&conn->addr); ptr = rawmsg; fragoffset = 0; hlen = cw_get_hdr_hlen(rawmsg)*4; mtu = conn->mtu; while (packetlen>mtu){ cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F,1); cw_set_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,mtu,(struct sockaddr*)&conn->addr); /* if (conn->write_data(conn,ptr,mtu)<0)*/ return -1; /* // XXX Fragmentation stuff..*/ ptr +=mtu-hlen; fragoffset+=(mtu-hlen)/8; packetlen-=mtu-hlen; } if (fragoffset) cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F | CAPWAP_FLAG_HDR_L,1); else cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F,0); cw_set_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,packetlen,(struct sockaddr*)&conn->addr); /*// return conn->write_data(conn,ptr,packetlen-0);*/ return 0; }
int netconn_send_capwap_msg(struct netconn * nc, uint8_t *rawmsg, int msglen) { /* int msglen = cw_get_hdr_msg_total_len(rawmsg);*/ uint8_t * ptr = rawmsg; int fragoffset = 0; int hlen = cw_get_hdr_hlen(rawmsg)*4; /* int mtu = nc->mtu; */ int mtu = 1400; while (msglen>mtu){ cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F,1); cw_set_dword(ptr+4, nc->fragid<<16 | fragoffset<<3 ); /*/// cw_dbg_pkt_nc(DBG_PKT_OUT,nc,ptr,mtu,(struct sockaddr*)&nc->addr);*/ if (nc->write(nc,ptr,mtu)<0) return -1; ptr +=mtu-hlen; fragoffset+=(mtu-hlen)/8; msglen-=mtu-hlen; } if (fragoffset) cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F | CAPWAP_FLAG_HDR_L,1); else cw_set_hdr_flags(rawmsg,CAPWAP_FLAG_HDR_F,0); cw_set_dword(ptr+4, nc->fragid<<16 | fragoffset<<3 ); /*// cw_dbg_pkt_nc(DBG_PKT_OUT,nc,ptr,msglen,(struct sockaddr*)&nc->addr);*/ return nc->write(nc,ptr,msglen-0); }
int hdr_print(char *str, uint8_t *packet, int len) { char *s = str; if (len<1){ s+=sprintf(s,"\tNo info available."); return s-str; } int preamble = cw_get_hdr_preamble(packet); //CWTH_GET_PREAMBLE(packet); if (preamble==01){ s+=sprintf(s,"\tEncrypted data."); return s-str; } if (preamble!=00){ s+=sprintf(s,"\tWrong CAPWAP version or encryption type."); return s-str; } if (len < 4){ s+=sprintf(s,"\tNo more data. Packet too short."); return s-str; } int hlen = cw_get_hdr_hlen(packet); //CWTH_GET_HLEN(packet); int rid = cw_get_hdr_rid(packet); int wbid = cw_get_hdr_wbid(packet); s+=sprintf(s,"\tHLEN: %d, RID: %02X, WBID %02X",hlen,rid,wbid); s+=sprintf(s," Flags: (T=%d,F=%d,L=%d,W=%d,M=%d,K=%d)\n", cw_get_hdr_flag_t(packet), cw_get_hdr_flag_f(packet), cw_get_hdr_flag_l(packet), cw_get_hdr_flag_w(packet), cw_get_hdr_flag_m(packet), cw_get_hdr_flag_k(packet) ); if (cw_get_hdr_flag_m(packet)){ uint8_t * rmac = cw_get_hdr_rmac(packet); s+=sprintf(s,"\tRadio MAC: %s\n",sock_hwaddr2str(bstr_data(rmac), bstr_len(rmac))); } if (len < 8){ s+=sprintf(s,"\tNo more data. Packet too short."); return s-str; } int frag_id = cw_get_hdr_fragid(packet); int frag_offs = cw_get_hdr_fragoffset(packet); s+=sprintf(s,"\tFrag Id: %d, Frag Offs:: %d\n",frag_id,frag_offs); int bhlen = 4*hlen; if (cw_get_hdr_flag_f(packet) && frag_offs!=0){ s+=sprintf(s,"\tFragment data ..."); return s-str; } if (len<bhlen+4){ s+=sprintf(s,"\tNo more data. Packet too short."); return s-str; } int msgtype = ntohl(*((uint32_t*)(packet+bhlen))); s+=sprintf(s,"\tMsgType: %d",msgtype); if (len<bhlen+8){ s+=sprintf(s,"\n\tNo more data. Packet too short."); return s-str; } int seqnum = (ntohl(*((uint32_t*)(packet+bhlen+4))))>>24; int msgelemlen = 0xFF & ((ntohl(*((uint32_t*)(packet+bhlen+4))))>>8); s+=sprintf(s,", SeqNum: %d, MsgelemLen:%d",seqnum,msgelemlen); return s-str; }
/** * Format a Packet Header */ int cw_format_pkt_hdr(char *dst,int level,struct conn *conn, uint8_t * packet, int len,struct sockaddr *from) { char *s=dst; switch (level) { case DBG_PKT_IN: if (cw_get_hdr_flag_f(packet)){ s+=sprintf(s,"Fragment from %s",sock_addr2str_p(from)); } else{ s+=sprintf(s,"From %s",sock_addr2str_p(from)); } break; case DBG_PKT_OUT: if (cw_get_hdr_flag_f(packet)){ s+=sprintf(s,"Fragment to %s",sock_addr2str(from)); } else{ s+=sprintf(s,"To %s",sock_addr2str(from)); } break; } s+=sprintf(s," l=%d: ",len); int preamble = cw_get_hdr_preamble(packet); if (preamble==01){ s+=sprintf(s," (encrypted)"); return s-dst; } if (len<4) goto abort; /* if (cw_get_hdr_flag_f(packet)){ s+=sprintf(s," (fragmented)"); } */ int hlen = cw_get_hdr_hlen(packet); int rid = cw_get_hdr_rid(packet); int wbid = cw_get_hdr_wbid(packet); s+=sprintf(s," H:%d R:%02d W:%02d",hlen,rid,wbid); s+=sprintf(s," Flgs:"); s+=format_hdr_flags(s,packet); if (len<8) goto abort; int frag_id = cw_get_hdr_fragid(packet); int frag_offs = cw_get_hdr_fragoffset(packet); s+=sprintf(s," Frag/Offs:%d/%d",frag_id,frag_offs); if (cw_get_hdr_flag_m(packet)) { /* rmac is present, print the rmac */ int rmac_len=cw_get_hdr_rmac_len(packet); int plen=rmac_len; if (rmac_len+8>len) plen=len-8; if (rmac_len>10) plen=10; s+=sprintf(s," R-MAC:"); s+=format_mac(s,cw_get_hdr_rmac_data(packet),plen); if (rmac_len>10){ s+=sprintf(s," ... (len=%d)",rmac_len); } } if (cw_get_hdr_flag_w(packet)){ /* print wireless specific info */ int ws_len = cw_get_hdr_ws_len(packet); int plen = ws_len > 20 ? 20:ws_len; s+=sprintf(s," WS:"); s+=format_hexu(s,cw_get_hdr_ws_data(packet),plen); if (ws_len>20){ s+=sprintf(s," ... (len=%d)",ws_len); } } return s-dst; abort: s+=sprintf(s," Incomplete..."); return s-dst; }
int conn_send_msg(struct conn * conn, uint8_t *rawmsg) { int packetlen = cw_get_hdr_msg_total_len(rawmsg); cw_dbg_msg(DBG_MSG_OUT, conn,rawmsg, packetlen,(struct sockaddr*)&conn->addr); //uint8_t * msgptr = rawmsg + cw_get_hdr_msg_offset(rawmsg); /* Zyxel doesn't count msg element length from behind seqnum */ if (conn->capwap_mode == CW_MODE_ZYXEL){ // XXX val-=3; } uint8_t * ptr = rawmsg; int fragoffset = 0; int hlen = cw_get_hdr_hlen(rawmsg)*4; //cw_set_hdr_hlen(rawmsg,223); //*(rawmsg + 8)=99; //cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_M,1); int mtu = conn->mtu; while (packetlen>mtu){ cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F,1); cw_put_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,mtu,(struct sockaddr*)&conn->addr); //XXX { // char h[200]; // hdr_print(h,ptr,mtu); // cw_dbg(DBG_PKT_OUT,"Sending capwap packet to %s:\n%s",sock_addr2str(&conn->addr),h); } // cw_dbg_dmp(DBG_PKT_DMP,ptr,mtu,"Sending packet ..."); if (conn->write(conn,ptr,mtu)<0) return -1; ptr +=mtu-hlen; fragoffset+=(mtu-hlen)/8; packetlen-=mtu-hlen; //XXX if (hlen>8) // memcpy(ptr+8,cwmsg->ctrlhdr+8,hlen-8); } // val = (preamble << 24) | ((hlen/4)<<19) | (cwmsg->rid<<14) |(wbid<<9) | /*CWTH_FLAGS_T|*/ //cwmsg->flags; //printf("VAL = %08x, %08x\n",val,cwmsg->flags); //printf("FRag offset :::::::::::::::::::::::::::: %d\n",fragoffset); if (fragoffset) cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F | CW_FLAG_HDR_L,1); else cw_set_hdr_flags(rawmsg,CW_FLAG_HDR_F,0); // printf("Setting first byte %08X\n",val); // *((uint32_t*)ptr)=htonl(val); // val = conn->fragid<<16 | fragoffset<<3; // *((uint32_t*)(ptr+4))=htonl(val); cw_put_dword(ptr+4, conn->fragid<<16 | fragoffset<<3 ); { // char h[1024]; // hdr_print(h,ptr,msglen-fragoffset*8+hlen); // cw_dbg(DBG_CW_PKT_OUT,"Sending capwap packet to %s:\n%s",sock_addr2str(&conn->addr),h); } // cw_dbg_dmp(DBG_PKT_DMP,ptr,packetlen,"Sending packet ..."); //return conn->write(conn,ptr,msglen-fragoffset*8+hlen); //printf("Send packet len %p %d\n",ptr,packetlen); cw_dbg_pkt(DBG_PKT_OUT,conn,ptr,packetlen,(struct sockaddr*)&conn->addr); return conn->write(conn,ptr,packetlen-0); }