1 //----------------------------------------------------------------------- 2 // This software is part of the OpenBeOS distribution and is covered 3 // by the OpenBeOS license. 4 // 5 // Copyright (c) 2003-2004 Waldemar Kornewald, Waldemar.Kornewald@web.de 6 //----------------------------------------------------------------------- 7 8 #include "DiscoveryPacket.h" 9 10 #include <core_funcs.h> 11 12 13 DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID = 0x0000) 14 : fCode(code), 15 fSessionID(sessionID), 16 fInitStatus(B_OK) 17 { 18 } 19 20 21 DiscoveryPacket::DiscoveryPacket(struct mbuf *packet, uint32 start = 0) 22 { 23 // decode packet 24 uint8 *data = mtod(packet, uint8*); 25 data += start; 26 pppoe_header *header = (pppoe_header*) data; 27 28 SetCode(header->code); 29 30 uint16 length = ntohs(header->length); 31 32 if(length > packet->m_len - PPPoE_HEADER_SIZE - start) { 33 fInitStatus = B_ERROR; 34 return; 35 // there are no tags (or one corrupted tag) 36 } 37 38 int32 position = 0; 39 pppoe_tag *tag; 40 41 while(position <= length - 4) { 42 tag = (pppoe_tag*) (header->data + position); 43 position += ntohs(tag->length) + 4; 44 45 AddTag(ntohs(tag->type), tag->data, ntohs(tag->length)); 46 } 47 48 fInitStatus = B_OK; 49 } 50 51 52 DiscoveryPacket::~DiscoveryPacket() 53 { 54 for(int32 index = 0; index < CountTags(); index++) 55 free(TagAt(index)); 56 } 57 58 59 bool 60 DiscoveryPacket::AddTag(uint16 type, const void *data, uint16 length, int32 index = -1) 61 { 62 pppoe_tag *add = (pppoe_tag*) malloc(length + 4); 63 add->type = type; 64 add->length = length; 65 memcpy(add->data, data, length); 66 67 bool status; 68 if(index < 0) 69 status = fTags.AddItem(add); 70 else 71 status = fTags.AddItem(add, index); 72 if(!status) { 73 free(add); 74 return false; 75 } 76 77 return true; 78 } 79 80 81 bool 82 DiscoveryPacket::RemoveTag(pppoe_tag *tag) 83 { 84 if(!fTags.HasItem(tag)) 85 return false; 86 87 fTags.RemoveItem(tag); 88 free(tag); 89 90 return true; 91 } 92 93 94 pppoe_tag* 95 DiscoveryPacket::TagAt(int32 index) const 96 { 97 pppoe_tag *tag = fTags.ItemAt(index); 98 99 if(tag == fTags.GetDefaultItem()) 100 return NULL; 101 102 return tag; 103 } 104 105 106 pppoe_tag* 107 DiscoveryPacket::TagWithType(uint16 type) const 108 { 109 pppoe_tag *tag; 110 111 for(int32 index = 0; index < CountTags(); index++) { 112 tag = TagAt(index); 113 if(tag && tag->type == type) 114 return tag; 115 } 116 117 return NULL; 118 } 119 120 121 struct mbuf* 122 DiscoveryPacket::ToMbuf(uint32 MTU, uint32 reserve = ETHER_HDR_LEN) 123 { 124 struct mbuf *packet = m_gethdr(MT_DATA); 125 packet->m_data += reserve; 126 127 pppoe_header *header = mtod(packet, pppoe_header*); 128 129 header->version = PPPoE_VERSION; 130 header->type = PPPoE_TYPE; 131 header->code = Code(); 132 header->sessionID = SessionID(); 133 134 uint16 length = 0; 135 pppoe_tag *tag; 136 137 for(int32 index = 0; index < CountTags(); index++) { 138 tag = TagAt(index); 139 140 // make sure we have enough space left 141 if(MTU - length < tag->length) { 142 m_freem(packet); 143 return NULL; 144 } 145 146 *((uint16*)(header->data + length)) = htons(tag->type); 147 length += 2; 148 *((uint16*)(header->data + length)) = htons(tag->length); 149 length += 2; 150 memcpy(header->data + length, tag->data, tag->length); 151 length += tag->length; 152 } 153 154 header->length = htons(length); 155 packet->m_pkthdr.len = packet->m_len = length + PPPoE_HEADER_SIZE; 156 157 return packet; 158 } 159