static int receive_msg(int fd, char *rx, size_t size,
unsigned int timeout, bool debug)
{
fd_set fds;
struct timeval tv;
int ret;
unsigned int count;
/* Initialize structures for select */
FD_ZERO(&fds);
FD_SET(fd, &fds);
/*
* Microcontrolle answers very fast,
* Timeout is just to take care if no answer is
* sent
*/
tv.tv_sec = timeout;
tv.tv_usec = 0; /* Check for not more as 10 mSec */
ret = select(fd + 1, &fds, NULL, NULL, &tv);
if (ret == 0) {
ERROR("Timeout, no answer from microcontroller");
return -EPROTO;
}
ret = read(fd, rx, size);
if (ret < 3) {
ERROR("Error in read: %d", ret);
return -EBADMSG;
}
count = ret;
if (debug)
dump_ascii(true, rx, count);
/*
* Try some syntax check
*/
if (rx[0] != '$') {
ERROR("First byte is not '$' but '%c'", rx[0]);
return -EBADMSG;
}
if (!verify_chksum(rx, &count)) {
return -EBADMSG;
}
return 0;
}
char* layer2(char* in, char dir) {
debug("********** LAYER 2 **********");
size_t len = strlen(in);
debug("%zi bytes, direction: %c ", len, dir);
char *out = NULL;
if (dir == 's') { // Send data
int nfrm = len/(FS-FSH) + (len % (FS-FSH) ? 1 : 0);
debug("%i frame(s) detected", nfrm);
// Allocate twice the frame size for max amount of DLEs
if(!(out = calloc((nfrm)*FS*2, sizeof(char)))) {
perror("Could not allocate memory for transmission "
"buffer");
exit(2);
};
int opos = 0;
for (int seg = 0, addcount = 0; seg < nfrm; seg++) { // Construct ech frame
size_t count = seg < nfrm -1 || !(len % (FS-FSH)) ? FS : seg == 0 ? len + FSH : (len % (FS-FSH))+FSH;
out[opos] = STX; opos++; //STX
int countpos = opos; opos += 3;
out[opos] = '0'; opos++; //ACK
sprintf(out+opos,"%1i",seg % 10); opos++; //Seq#
out[opos] = seg < nfrm -1 ? '0' : '1'; opos++; //LFrame
for (int ipos = 0; ipos < count-FSH; ipos++) {
char c = in[seg*(FS-FSH)+ipos];
if (c == STX || c == ETX || c == DLE) {
out[opos++] = DLE; addcount++;
}
out[opos++] = c;
}
// Now put the real count in
char c = out[countpos+3];
sprintf(out+countpos,"%03li", count+addcount); //Count
out[countpos+3] = c;
sprintf(out+opos, "%04x", chksum(out+countpos, out+opos)); opos += 4; //Chksum
out[opos] = ETX; opos++; //ETX
//debug("%zi, %i, %s", count, addcount, out);
addcount = 0;
}
} else { // Receive data
ptrdiff_t off = 0, count; // Beginning of frame (STX)
while(in[off] == STX){
if ((count = verify_chksum(in+off)) < 0) exit(1);
else off+= count;
}
char* destuff = calloc(len + 1, sizeof(char));
int opos;
char c;
for (int ipos = opos = 0; ipos < len; ipos++) {
if ((c = in[ipos]) == DLE) {
if (in[ipos+1] == DLE) {
destuff[opos] = c;
opos++; ipos++;
}
continue;
}
else {
destuff[opos] = c;
opos++;
}
}
len = strlen(destuff);
int nfrm = len/FS + (len % FS ? 1 : 0);
debug("%i frame(s) detected", nfrm);
out = calloc(nfrm*FS, sizeof(char));
for (int seg = 0; seg < nfrm; seg++) {
size_t count = seg < nfrm -1 || !(len % FS) ? (MSS+SSH) : seg == 0 ? len - FSH : (len % FS) - FSH;
strncpy(out+seg*(FS-FSH),destuff+seg*FS+7, count); // Strip header, trailer
}
free(destuff);
}
free(in);
debug("Current buffer '''%s'''", out);
debug("*****************************");
return out;
}
/**
* get_packet()
* ------------
* a wrapper function for the pkt_card_* interface, returning the
* packet information about packet size, mac, network, transport layer
* info, as well as error status.
* returns: struct packet_info *
**/
struct packet_info *get_packet (struct SETTINGS *mySettings)
{
struct sockaddr_ll ifinfo; // interface info
int wlan_ng_hdr_len = sizeof(wlan_ng_hdr_t);
int pream_size = sizeof(struct airids_frame_info);
int wlan_hdr_len = sizeof(wlan_hdr_t);
int prism2_hdr_len = sizeof(prism2_hdr_t);
int recvlen = 0;
int ip_len = 0;
int tcp_len = 0;
int offset = 0;
int total_hlen = 0;
char proto[2];
memset(msgbuf, 0, MAX_BUFFER_SIZE);
memset(packet, 0, sizeof(struct packet_info));
recvlen = pkt_card_sock_read(mySettings->sniff_socket, msgbuf, MAX_BUFFER_SIZE, &ifinfo);
if (DEBUG) fprintf(stderr,"total packet size: %d\n",recvlen);
if (recvlen < 1){
return (NULL);
}
if(mySettings->signal_support){
packet->driver_proto = AIRONET_MOD;
packet->packet_size = recvlen - pream_size;
/* cast into airids_frame_info */
packet->driver_pkt = (struct airids_frame_info *)msgbuf;
if(packet->driver_pkt->fcs_error){
packet->error_status = FCS_ERR;
return (packet);
}
offset = pream_size;
}
else{
packet->packet_size = recvlen;
offset = 0;
}
if(mySettings->card_type == AIRONET){
packet->mac_proto = p802_11;
packet->mac_pkt = (void *) (msgbuf + offset);
/** simple test to make sure there are more higher protos **/
if (packet->packet_size < (wlan_hdr_len + 3)){
return (packet);
}
proto[0] = msgbuf[offset+wlan_hdr_len];
proto[1] = msgbuf[offset+wlan_hdr_len+1];
}
else if(mySettings->card_type == PRISMII){
packet->mac_proto = hfa384x;
packet->mac_pkt = (void *) (msgbuf + offset);
/** simple test to make sure there are more higher protos **/
if (packet->packet_size < (prism2_hdr_len + 3)){
return (packet);
}
/** don't include driver crap in size byte analysis later **/
packet->packet_size -= (sizeof(hfa384x_descript_t)+(sizeof(__u8)*7));
proto[0] = msgbuf[offset+prism2_hdr_len];
proto[1] = msgbuf[offset+prism2_hdr_len+1];
}
else if((mySettings->card_type==HERMES)||(mySettings->card_type==HOSTAP)||(mySettings->card_type==WLANNG)){
packet->mac_proto = wlanngp2;
packet->mac_pkt = (void *) (msgbuf + offset);
/** simple test to make sure there are more higher protos **/
if (packet->packet_size < (wlan_ng_hdr_len + 3)){
return (packet);
}
/** don't include driver crap in size byte analysis later **/
packet->packet_size -= wlan_ng_hdr_len;
proto[0] = msgbuf[offset+wlan_ng_hdr_len+wlan_hdr_len];
proto[1] = msgbuf[offset+wlan_ng_hdr_len+wlan_hdr_len+1];
}
else{
/** shouldn't ever get here... **/
fprintf(stderr,"card type unsupported!\n");
exit(1);
}
/** this doesn't work... **/
// ifinfo.sll_protocol = htons(ifinfo.sll_protocol);
/**
* lets see if its IP... sockaddr_ll only for 802.3! :(
* +2 at the end because of protocol identifier between 802.11
* protocol and ip header...
**/
/** see if its IP packet (there should be a better method...)**/
if ((proto[0] == 8) && (proto[1] == 0)){
if (mySettings->card_type == AIRONET){
packet->net_pkt = (void *) (msgbuf + offset + wlan_hdr_len + 2);
total_hlen += offset + wlan_hdr_len + 2;
}
else if (mySettings->card_type == PRISMII){
packet->net_pkt = (void *) (msgbuf + offset + prism2_hdr_len + 2);
total_hlen += offset + prism2_hdr_len + 2;
}
else if ((mySettings->card_type == HERMES)||(mySettings->card_type == HOSTAP)||(mySettings->card_type == WLANNG)){
packet->net_pkt = (void *) (msgbuf + offset + wlan_ng_hdr_len + wlan_hdr_len + 2);
total_hlen += offset + wlan_ng_hdr_len + wlan_hdr_len + 2;
}
else{
fprintf(stderr, "major internal error!");
exit(1);
}
/** if its version 4, then we're okay **/
if ( ((struct iphdr *)packet->net_pkt)->version == 4){
packet->net_proto = IPv4;
/** if iphdr is invalid, then lets just ignore the entire packet **/
if (!verify_chksum((struct iphdr *)packet->net_pkt)){
packet->error_status = IPCHKSUM_ERR;
return (packet);
}
/** grab the higer level protocols **/
ip_len = ((struct iphdr *)packet->net_pkt)->ihl * 4;
packet->trans_pkt = (void *) (packet->net_pkt + ip_len);
total_hlen += ip_len;
if ((total_hlen > MAX_BUFFER_SIZE)||(total_hlen > recvlen)){
packet->error_status = IPHDRLEN_ERR;
return (packet);
}
/** parse higher protocols here... **/
if (((struct iphdr *)packet->net_pkt)->protocol == IPPROTO_TCP){
packet->trans_proto = TCP;
tcp_len = ((struct tcphdr *)packet->trans_pkt)->doff * 4;
packet->data = (void *) (packet->trans_pkt + tcp_len);
total_hlen += tcp_len;
packet->data_size = recvlen - total_hlen;
}
else if (((struct iphdr *)packet->net_pkt)->protocol == IPPROTO_UDP){
packet->trans_proto = UDP;
}
else if (((struct iphdr *)packet->net_pkt)->protocol == IPPROTO_ICMP){
packet->trans_proto = ICMP;
}
else{
packet->trans_proto = OTHER;
}
}
else if ( ((struct iphdr *)packet->net_pkt)->version == 6){
packet->net_proto = IPv6;
}
else{
packet->net_proto = OTHER;
}
}
else{
packet->net_proto = OTHER;
}
return(packet);
}