l2cap.cc

NS2网络仿真软件是目前最为流行的网络仿真模拟软件

		if (cid->total_length) 
			Packet::free(cid->packet);
		cid->packet = p;
		cid->length = l2cap->length;
		cid->total_length = ch->size() - 4;
	}
	// continuation of L2CAP packet
	else {
		cid->total_length += ch->size();	
		Packet::free(p);
	}
	// L2CAP packet complete
	if (cid->total_length == cid->length) {
		printf("L2CA_DATA_READ\t\tSIZE: %-10d  CID: %d\n",cid->length, cid->lcid);
		cid->length = 0;
		BTHost* bthost = (BTHost*)uptarget_;
		hdr_cmn* hdr = HDR_CMN(cid->packet);
		hdr->size() = cid->total_length;
		bthost->dataRead(cid->packet, cid);
		cid->packet = 0;
		cid->total_length = 0;
	}
}	
		
// Data Write primitive
void
L2CAP::dataWrite (Packet* p, uchar lcid)
{
	WriteReq* req = new WriteReq;
	CID_attr* cid = findCID(lcid);	
	uchar ch = cid->ch;
	req->cid = lcid;
	req->pkt = p;
	write_reqQ_[ch].push(req);
	if (callback_[ch]) {
		callback_[ch]=0;
		callback(ch);
	}
}

// segment and send L2CAP packet to lower layer
void
L2CAP::callback(uchar ch)
{
	if (write_reqQ_[ch].empty()) {
		callback_[ch] = 1;
		return;
	}
	WriteReq* wr = write_reqQ_[ch].front();
	write_reqQ_[ch].pop();
	Packet* p = wr->pkt;	
	Packet* pkt;
	int lcid = wr->cid;

	// Find cid struct for the given local CID

	hdr_cmn* hdr = HDR_CMN(p);
	CID_attr* cid = findCID(lcid);
	int rcid = cid->rcid;
		
	// Find L2CAP packet size
	int length = hdr->size();
	hdr->size() += L2capHdrSize;

	// Find number of segments
	bt_packet_type packet_type = getPacketType(p, ch);
	int packetsize = Payload[packet_type];			
	int num_fullseg = (hdr->size()/packetsize);
	int last_segsize = (hdr->size()%packetsize);
	int num_seg = (last_segsize) ? num_fullseg+1 : num_fullseg; 
	hdr_bt* bt;
	hdr_l2cap* l2cap;

	// Simulate Segmentation 
	for (int i=0; i < num_seg; i++) {
		if (i == 0) {
			pkt = p->copy();
			l2cap = HDR_L2CAP(pkt);
			l2cap->length = length;
			bt = HDR_BT(pkt);
			bt->ph.l_ch = 0x0a;
		}
		else {
			pkt = Packet::alloc();
			bt = HDR_BT(pkt);
			bt->ph.l_ch = 0x01;
		}
			
		l2cap = HDR_L2CAP(pkt);
		hdr_cmn* hdr = HDR_CMN(pkt);
		bt->am_addr = ch+1;
		l2cap->cid = rcid;
		if (i < num_fullseg) {
			hdr->size() = packetsize;		
			bt->type = packet_type;
		}
		else {
			hdr->size() = last_segsize;
			bt->type = getPacketType(pkt,ch);
		}
		downtarget_->recv(pkt, this);
	}	
	Packet::free(p);
	delete wr;
}	


// get the packet type to be used
bt_packet_type 
L2CAP::getPacketType(Packet* p, uchar ch)
{
	hdr_cmn* hdr = HDR_CMN(p);
	int size = hdr->size();
	uchar fec = 0; 
	// Get the largest packet type for the connection 
	bt_packet_type largest = (packetType_[ch]) ? packetType_[ch] : BT_DH5;
	bt_packet_type type;
	if (largest == BT_DM1 || largest == BT_DM3 || largest == BT_DM5)
		fec = 1;
	
	if (fec) {
		// FEC required
		if ((size > Payload[14]) || ((size <= Payload[14]) && (size > Payload[10])))
			type = BT_DM5;
		else if (size <= Payload[10] && size > Payload[3])
			type = BT_DM3;
		else 
			type = BT_DM1;
	}
	else {
		// No FEC
		if ((size > Payload[15]) || ((size <= Payload[15]) && (size > Payload[11])))
			type = BT_DH5;
		else if (size <= Payload[11] && size > Payload[4])
			type = BT_DH3;
		else 
			type = BT_DH1;
	}	
	type = (type > largest) ? largest: type;
		
	return type;
}


// Connect response primitive
void
L2CAP::L2CA_ConnectRsp (CID_attr* cid, uchar result)
{
	if (!result) {
		sendL2CAP_ConnectRsp (cid, 0x0004);
		delete cid;
	}
	else {
		cid->state = OPEN;
		cid_list_vec_[cid->ch].push_back(cid);
		sendL2CAP_ConnectRsp (cid, 0);
	}	
}

// create and send L2CAP_ConnectRequest
void 
L2CAP::sendL2CAP_ConnectRequest (int ch, uchar scid, uchar id, uchar psm)
{
	Packet* p = Packet::alloc(4);
	hdr_cmn* hdr = HDR_CMN(p);
	hdr_l2cap* l2cap = HDR_L2CAP(p);
	hdr_bt* bt = HDR_BT(p);
	bt->am_addr = ch+1;
	bt->ph.l_ch = 0x0a;
	bt->type = BT_DM1;
	L2cmd& l2cmd = l2cap->cmd; 	
	uchar* data = p->accessdata();

	CID_attr* cid_attr = new CID_attr();
	cid_attr->lcid = scid;
	cid_attr->psm = psm;
	cid_attr->ch = ch;
	cid_attr->state = W4_L2CAP_ConnectRsp;	
	cid_list_vec_[ch].push_back(cid_attr);

	// Set fields of the L2CAP header
	l2cap->length = 12;		// Check
	hdr->size() = 12;
	l2cap->cid = 0x01;

	// Set fields in Connect Request Command 
	l2cmd.length = 0x0004;
	l2cmd.code = 0x02;
	l2cmd.identifier = ++identifier_;
	data[0] = psm;
	data[1] = scid;
	downtarget_->recv(p, this);
}	
	
// create and send L2CAP_ConnectRsp
void
L2CAP::sendL2CAP_ConnectRsp (CID_attr* cid, uchar result)
{
	Packet* p = Packet::alloc(8);
	hdr_l2cap* l2cap = HDR_L2CAP(p);
	hdr_bt* bt = HDR_BT(p);
	hdr_cmn* ch = HDR_CMN(p);
	bt->am_addr = cid->ch+1;
	bt->ph.l_ch = 0x0a;
	bt->type = BT_DM1;
	ch->size() = Payload[bt->type];
	L2cmd& l2cmd = l2cap->cmd; 	
	uchar* data = p->accessdata();

	// Set fields of the L2CAP header
	l2cap->length = 16; 	
	l2cap->cid = 0x01;

	// Set fields in Connect Request Command 
	l2cmd.length = 0x0008;
	l2cmd.code = 0x03;
	l2cmd.identifier = cid->identifier;
	data[0] = cid->rcid;
	data[1] = cid->lcid;
	data[2] = result;
	downtarget_->recv(p, this);
}
	
// find CID given the connection handle 
CID_attr*
L2CAP::findCID (uchar cid, uchar connection_handle)
{	
	CID_attr* cid_attr = 0;
	list<CID_attr*>::iterator iter; 
	for (iter = cid_list_vec_[connection_handle].begin(); iter != cid_list_vec_[connection_handle].end(); iter++) {
		CID_attr* cid_attr = (CID_attr*)(*iter);
		if (cid_attr->lcid == cid) 
			return (CID_attr*)(*iter);
	}
	return 0;
}	

// find CID without connection handle information 
CID_attr*
L2CAP::findCID (uchar cid)
{	
	CID_attr* cid_attr = 0;
	list<CID_attr*>::iterator iter; 
	for (int i=0; i<7; i++) {
		for (iter = cid_list_vec_[i].begin(); iter != cid_list_vec_[i].end(); iter++) 
		{
			CID_attr* cid_attr = (CID_attr*)(*iter);
			if (cid_attr->lcid == cid) 
				return (CID_attr*)(*iter);
		}
	}
	return 0;
}