void response_split_task(void * arg)
{
    client_info_t * client = arg;

    DBG_FUNC_ENTER();
    DBG_VERBOSE("%s: client: %p\n", __FUNCTION__, client);
    /* variables for handling response split */
    char temp[1024];
    uint32_t temp_len = 0;
    read_stages_t stage = HEADER_LEN_READ;
    uint32_t payload_offset = 0;
    response_t * resp = NULL;
    uint32_t rem_size = 0;
    txn_buf_t * cur_buf = NULL;

    while (1) {
        if (cur_buf != NULL) {
            DBG_ALLOC("%s: FREE cur_buf->buf: %p cur_buf: %p\n", __FUNCTION__, cur_buf->buf, cur_buf);
            free(cur_buf->buf);
            free(cur_buf);
            cur_buf = NULL;
        }
        cur_buf = (txn_buf_t *)queue_pop(client->buf_q);
        DBG_VERBOSE("%s: buf: %p len: %ld offset: %ld\n", __FUNCTION__, cur_buf->buf, cur_buf->len, cur_buf->offset);
        /* write a response split using fixed len */
        rem_size = cur_buf->len - cur_buf->offset;
        DBG_VERBOSE("rem_size: %u\n", rem_size);
        while ((temp_len > 0) && (rem_size > 0)) {
            switch(stage) {
                case HEADER_LEN_READ:
                    {
                        uint32_t hdr_len = 0;
                        if ((rem_size + temp_len) < HEADER_SIZE) {
                            memcpy(temp + temp_len, cur_buf->buf + cur_buf->offset, rem_size);
                            temp_len += rem_size;
                            rem_size = 0;
                        } else {
                            resp = malloc(sizeof(response_t));
                            assert(resp != NULL);
                            DBG_ALLOC("%s: ALLOC request: %p\n", __FUNCTION__, resp);
                            resp->buf = NULL;
                            if (temp_len < HEADER_SIZE) {
                                memcpy((uint8_t *)&hdr_len, temp, temp_len);
                                memcpy(((uint8_t *)&hdr_len) + temp_len, cur_buf->buf + cur_buf->offset, HEADER_SIZE - temp_len);
                                DBG_VERBOSE("HEADER length: %u\n", hdr_len);
                                temp_len = 0;
                                rem_size -= HEADER_SIZE + temp_len;
                            } else {
                                memcpy((uint8_t *)&hdr_len, temp, HEADER_SIZE);
                                temp_len -= HEADER_SIZE;
                            }
                            resp->header_len = hdr_len;
                            stage = HEADER_READ;
                        }
                        break;
                    }
                case HEADER_READ:
                    {
                        if ((rem_size + temp_len) < resp->header_len) {
                            memcpy(temp + temp_len, cur_buf->buf + cur_buf->offset, rem_size);
                            temp_len += rem_size;
                            rem_size = 0;
                        } else  {
                            if (temp_len < resp->header_len) {
                                memcpy((uint8_t *)&resp->hdr, temp, temp_len);
                                memcpy(((uint8_t *)&resp->hdr) + temp_len, cur_buf->buf + cur_buf->offset, resp->header_len - temp_len);
                                temp_len = 0;
                                rem_size -= (resp->header_len - temp_len);
                            } else {
                                memcpy((uint8_t *)&resp->hdr, temp, resp->header_len);
                                DBG_VERBOSE("REQUEST id: %u\n", resp->hdr.id);
                                temp_len -= resp->header_len;
                            }
                            stage = PAYLOAD_READ;
                            resp->buf = malloc(resp->hdr.len);
                            assert(resp->buf != NULL);
                        }
                        break;
                    }
                case PAYLOAD_READ:
                    {
                        if (temp_len < resp->hdr.len) {
                            memcpy(resp->buf, temp, temp_len);
                            temp_len = 0;
                        } 
                        break;
                    }
                default:
                    DBG_ERR("INVALID case needs to be find out");
                    assert(0);
                    break;
            }
        }
        while (rem_size > 0) {
            switch(stage) {
                case HEADER_LEN_READ:
                    if (rem_size < HEADER_SIZE){
                        memcpy(temp, cur_buf->buf + cur_buf->offset, rem_size);
                        temp_len = rem_size;
                        cur_buf->offset += rem_size;
                        rem_size = 0;
                    } else {
                        resp = malloc(sizeof(response_t));
                        assert(resp != NULL);
                        DBG_PRINT("Allocated resp: %p\n", resp);
                        resp->buf = NULL;

                        read_uint32_t((uint8_t *)cur_buf->buf + cur_buf->offset, HEADER_SIZE, &resp->header_len);
                        cur_buf->offset += HEADER_SIZE;
                        rem_size -= HEADER_SIZE;
                        DBG_VERBOSE("HEADER: %u\n", resp->header_len);
                        stage = HEADER_READ;
                    }
                    break;
                case HEADER_READ:
                    if (rem_size < resp->header_len) {
                        memcpy(temp, cur_buf->buf + cur_buf->offset, rem_size);
                        temp_len = rem_size;
                        cur_buf->offset += rem_size;
                        rem_size = 0;
                    } else {
                        read_pkt_hdr((uint8_t *)cur_buf->buf + cur_buf->offset, resp->header_len, &resp->hdr);
                        DBG_VERBOSE("HEADER: %x %x %x %x\n", resp->hdr.magic, resp->hdr.len, resp->hdr.id, resp->hdr.future);
                        rem_size -= resp->header_len;
                        resp->buf = malloc(resp->hdr.len);
                        assert(resp->buf != NULL);
                        DBG_ALLOC("ALLOC resp->buf: %p\n", resp->buf);
                        cur_buf->offset += resp->header_len;
                        stage = PAYLOAD_READ;
                    }
                    break;
                case PAYLOAD_READ:
                    {
                        size_t len_to_read = resp->hdr.len - payload_offset;
                        if (rem_size < len_to_read) {
                            memcpy(resp->buf + payload_offset, cur_buf->buf + cur_buf->offset, rem_size);
                            payload_offset += rem_size;
                            cur_buf->offset += rem_size;
                            rem_size = 0;
                        } else {
                            memcpy(resp->buf + payload_offset, cur_buf->buf + cur_buf->offset, len_to_read);
                            payload_offset = 0;
                            rem_size -= len_to_read;
                            cur_buf->offset += len_to_read;
                            /* request is done, lets push it into the queue */
                            DBG_VERBOSE("GOT THE RESPONSE for ID: %u\n", resp->hdr.id);
                            queue_push(client->res_q, (void *)resp);
                            resp = NULL;
                            stage = HEADER_LEN_READ;
                        }
                    }
                    break;
                default:
                    DBG_ERR("Invalid stage\n");
                    assert(0);
            }
        }
    }
    DBG_FUNC_EXIT();
}
Beispiel #2
0
int main(int argc, char **argv)
{
	if (argc != 2) {
		printf("usage: trans_pack file\n");
		exit(-1);
	}

	FILE *fp = NULL;
    size_t ret = 0, leftlen = 0, cnt = 0, bodylen = 0;
    uint8_t buf[65536], *u8p = NULL;

    fp = open_file(argv[1]);
    ret = read_pcap_hdr(fp, buf);
    print_pcap_hdr(buf);

    //Read Packet
    while ((ret = read_pkt_hdr(fp, buf)) > 0) {
        leftlen = bodylen =   ret;
        cnt++;
		printf("--------------------------\n");
        //if (cnt++ == 5) break;
        ret = read_pkt_body(fp, buf, leftlen);

        /// 不是IP的话直接跳过
        if (get_ether_type(buf) != ETHER_TYPE_IP) {
            //printf("It is Not IP\n");
            continue;
        } 

        /// 跳过Ethernet
        u8p = buf + sizeof(struct ether_header);
        leftlen = ret - sizeof(struct ether_header);
        if (get_ip_protocol(u8p) != IP_PROT_UDP) {
            // 不是UDP直接跳过
            //printf("It's not UDP\n");
            continue;
        } 

        /// 跳过IP和UDP
        u8p = u8p + sizeof(struct udphdr) + sizeof(struct iphdr);
        leftlen = leftlen - sizeof(struct udphdr) - sizeof(struct iphdr);
        /// 找到NS_UNIDATA数据包
        if (get_ns_type(u8p, leftlen) != NS_UNIDATA) {
            // 不是NS_UNIDATA数据包直接跳过
            //printf("Not NS-UNIDATA: %x\n", get_ns_type(u8p, leftlen));
            continue;
        } 

        /// 跳过NS
        u8p = u8p + sizeof(struct ns);
        leftlen = leftlen - sizeof(struct ns);
        uint8_t sdu_type = get_sdu_type(u8p, leftlen);
        if (sdu_type != DL_UNIDATA && sdu_type != UL_UNIDATA) {
            // 不是DL/UL格式,直接跳过
            // printf("Not DL/UL: %x\n", sdu_type);
            continue;
        }

        /// 不需要跳过SDU,直接处理ul/dl unidata
        if (sdu_type == DL_UNIDATA) {
            struct dl_unidata *dlp = (struct dl_unidata *)u8p;
            printf("dl type: %x\n", dlp->type);
            printf("dl tlli: %x %x %x %x\n", dlp->ctlli[0], dlp->ctlli[1], dlp->ctlli[2], dlp->ctlli[3]);
            // printf("dl liftime[0]: %x\n", dlp->lifetime[0]);
            //printf("dl mrac[0]: %x\n", dlp->mrac[0]);
            //printf("dl drx[0]: %x\n", dlp->drx[0]);
            //printf("dl ismi[0]: %x\n", dlp->imsi[0]);
            //printf("sizeof dl: %u\n", sizeof(struct dl_unidata));
            
            // 跳过dl_unidata固定头
            u8p += sizeof(struct dl_unidata);
            leftlen -= sizeof(struct dl_unidata);

            // 循环处理可变头
            uint8_t tmptype = 0;
            uint8_t tmplen = 0;
            while (*u8p != LLC_PDU) {
                tmptype = *u8p++; // 在这里将需要的字段提取出来
                tmplen = *u8p & 0x7f;
                // 跳过tmplen长度
                u8p = u8p + tmplen + 1;
                leftlen = leftlen - tmplen - 1;
            }
        } else {
            struct ul_unidata * ulp = (struct ul_unidata *)u8p;
            printf("ul type: %x\n", ulp->type);
            printf("ul tlli: %x %x %x %x\n", ulp->tlli[0], ulp->tlli[1], ulp->tlli[2], ulp->tlli[3]);

            /// 跳过固定UL_UNIDATA
            u8p = u8p + sizeof(struct ul_unidata);
            leftlen = leftlen - sizeof(struct ul_unidata);

            uint8_t tmptype = 0;
            uint8_t tmplen = 0;
            while (*u8p != LLC_PDU) {
                tmptype = *u8p++;
                tmplen = *u8p & 0x7f;
                // 跳过tmplen长度
                u8p = u8p + tmplen + 1;
                leftlen = leftlen - tmplen - 1;
            }
        }

        // 跳过LLC头,考虑变长
        u8p += 1; // 跳过ELEMENT ID
        leftlen -= 1;
		printf("llc pdu len %x\n", *u8p);

        if ((*u8p & 0x80) == 0) {
            u8p += 1;
            leftlen -= 1;
        }

        // 实际LLC PDU的长度
        uint8_t llcpdu_len = *u8p; 

        // 跳过LLC 长度字段
        u8p += 1;
        leftlen -= 1;

        if (get_llc_sapi(u8p) != LLGMM) {
            printf("Not LLGMM\n");
            continue;
        }
        /// 跳过LLC SAPI
        u8p += 1;
        leftlen -= 1;
        
        // 过滤非 Unconfirmed UI数据包
        if ((*u8p & 0xe0) != 0xc0) {
            continue;
        }
        u8p += 2;
        leftlen -= 2;

        struct gmm *gmmp = (struct gmm*)u8p;
        printf("-------[%d] len:%u\n", cnt, bodylen);
        u8p = u8p + sizeof(struct gmm);
        leftlen = leftlen - sizeof(struct gmm);
        switch (gmmp->type) {
            case ATTACH_REQUEST:
            {
                printf("It's attach request.\n");
                if (*(u8p+6)==0x05)
                {
                    struct attach_request_t *p_attach_req=(struct attach_request_t*)u8p;
                    printf("tmsi:%x%x%x%x\n",p_attach_req->tmsi[2],p_attach_req->tmsi[3],p_attach_req->tmsi[4],p_attach_req->tmsi[5]);
                }
                else if (*(u8p+6)==0x08)
                {
                    struct attach_request_i *p_attach_req=(struct attach_request_i*)u8p;
                    printf("imsi:%x%x%x%x%x%x%x%x\n",p_attach_req->imsi[1],p_attach_req->imsi[2],p_attach_req->imsi[3],p_attach_req->imsi[4],p_attach_req->imsi[5],p_attach_req->imsi[6],p_attach_req->imsi[7],p_attach_req->imsi[8]);
                }
                    //printf("imsi:%x%x%x%x%x%x%x%x\n",ntohs(p_attach_req->imsi[1]),ntohs(p_attach_req->imsi[2]),ntohs(p_attach_req->imsi[3]),ntohs(p_attach_req->imsi[4]),ntohs(p_attach_req->imsi[5]),ntohs(p_attach_req->imsi[6]),ntohs(p_attach_req->imsi[7]),ntohs(p_attach_req->imsi[8]));
            }
            break;
            case ATTACH_ACCEPT:
            {
                printf("It's attach accept.\n");
                struct attach_accept *p_attach_apt=(struct attach_accept*)u8p;
                printf("p-tmsi:%x%x%x%x\n",p_attach_apt->p_tmsi[3],p_attach_apt->p_tmsi[4],p_attach_apt->p_tmsi[5],p_attach_apt->p_tmsi[6]);

            }
            break;
            case ATTACH_COMPLETE:
            {
                
                printf("It's attach complete.\n");
            }
            break;
            case ATTACH_REJECT:
            {	
                
                printf("It's attach reject.\n");
                struct attach_reject *p_attach_rej=(struct attach_reject*)u8p;
                printf("%x\n",p_attach_rej->cause);
            }
            break;
            case DETACH_REQUEST:
            {
                printf("It's detach request.\n");
                printf("sizeof u8p is %d\n", llcpdu_len-8);
                if (llcpdu_len-8>1)
                {
                    struct detach_request *p_detach_req=(struct detach_request*)u8p;
                    printf("p_tmsi:%x%x%x%x\n.",p_detach_req->p_tmsi[3],p_detach_req->p_tmsi[4],p_detach_req->p_tmsi[5],p_detach_req->p_tmsi[6]);
                }
                else
                {
                    printf("It's a short one.\n");
                }
            }
            break;
            case DETACH_ACCEPT:
            {
                printf("It's detach accept.\n");
            }
            break;
            case ROUTE_UPDATE_REQUEST:
            {
                printf("It's RA update request.\n");
                struct ra_request *p_ra_req=(struct ra_request*)u8p;
                printf("%x\n",((p_ra_req->type)&0x07));
                //printf("%x\n",(p_ra_req->type)&0x07);
            }
            break;
            case ROUTE_UPDATE_ACCEPT:
            {
                struct ra_accept *p_ra_apt=(struct ra_accept*)u8p;
                printf("It's RA update accept.\n");
                printf("p-tmsi:%x%x%x%x\n",p_ra_apt->p_tmsi[3],p_ra_apt->p_tmsi[4],p_ra_apt->p_tmsi[5],p_ra_apt->p_tmsi[6]);
            }
            break;
            case ROUTE_UPDATE_COMPLETE:
            {
                
                printf("It's RA update complete.\n");
            }
            break;
            case ROUTE_UPDATE_REJECT:
            {
                printf("It's RA update reject.\n");
                struct ra_reject *p_ra_rej=(struct ra_reject*)u8p;
                printf("cause:%x\n",p_ra_rej->cause);
            }
            break;
                case ACTIVE_PDP_REQUEST:
            {       
                printf("It's pdp request\n");
                struct pdp_request *p_pdp_req=(struct pdp_request*)u8p;
                u8p=u8p+sizeof(struct pdp_request);
            
                
                uint8_t addr_len=*u8p;
                printf("addr len is:%x\n",addr_len);
                u8p=u8p+addr_len+1;
        
                
                if (llcpdu_len-14-addr_len>0)
                    if (*u8p==0x28)
                    {
                        uint8_t apn_len=*(++u8p);
                        u8p++;
                        //uint8_t apn[apn_len]=++u8p;
                        printf("apn:");
                        int i=0;
                        for (;i<apn_len;i++)
                        {
                            printf("%x",*(u8p+i));	
                        }
                        printf("\n");
                        
                    }			
            
            }
            break;
                case ACTIVE_PDP_ACCEPT:
            {
                printf("It's pdp accept\n");
            }
            break;
                case ACTIVE_PDP_REJECT:
            {
                printf("It's pdp reject\n");
                struct pdp_reject *p_pdp_rej=(struct pdp_reject*)u8p;
            }
            break;
            case DEACTIVE_PDP_REQUEST:
            {
                printf("It's deactivate pdp request.\n");
                struct pdp_deact_request *p_pdp_d_req=(struct pdp_deact_request*)u8p;
                printf("cause:%x\n",p_pdp_d_req->cause);
            }
            break;
            case DEACTIVE_PDP_ACCEPT:
            {
                printf("It's deactivate pdp accept.\n");
            }
            break;
            default:
                printf("type is: 0x%x\n", gmmp->type);
                break;
        }

        // test hash key 
        uint8_t keybuf1[20], keybuf2[20];
        gen_hash_key(buf, bodylen, keybuf1, keybuf2, 20);
        printf("Hash key: %s %s\n", keybuf1, keybuf2);
    }
    
	return 0;
}