1 /* 2 * Copyright 2003-2004, Waldemar Kornewald <wkornew@gmx.net> 3 * Distributed under the terms of the MIT License. 4 */ 5 6 #include <ByteOrder.h> 7 #include <NetBufferUtilities.h> 8 #include <net_buffer.h> 9 10 #include "DiscoveryPacket.h" 11 12 DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID) 13 : fCode(code), 14 fSessionID(sessionID), 15 fInitStatus(B_OK) 16 { 17 } 18 19 20 DiscoveryPacket::DiscoveryPacket(net_buffer *packet, uint32 start) 21 { 22 // decode packet 23 NetBufferHeaderReader<pppoe_header> bufferheader(packet); 24 if (bufferheader.Status() != B_OK) { 25 dprintf("NetBufferHeaderReader create fail\n"); 26 fInitStatus = B_ERROR; 27 return; 28 } 29 pppoe_header &header = bufferheader.Data(); 30 31 SetCode(header.code); 32 33 uint16 length = ntohs(header.length); 34 35 if(length > packet->size - PPPoE_HEADER_SIZE) { 36 dprintf("packet size less than pppoe payload\n"); 37 dprintf("pppoe payload: %d\n", length); 38 dprintf("PPPoE_HEADER_SIZE: %d\n", PPPoE_HEADER_SIZE); 39 dprintf("packet->size: %" B_PRIu32 "\n", packet->size); 40 fInitStatus = B_ERROR; 41 return; 42 // there are no tags (or one corrupted tag) 43 } 44 45 int32 position = 0; 46 pppoe_tag *tag; 47 48 while(position <= length - 4) { 49 tag = (pppoe_tag*) (header.data + position); 50 position += ntohs(tag->length) + 4; 51 52 AddTag(ntohs(tag->type), tag->data, ntohs(tag->length)); 53 } 54 55 fInitStatus = B_OK; 56 } 57 58 59 DiscoveryPacket::~DiscoveryPacket() 60 { 61 for(int32 index = 0; index < CountTags(); index++) 62 free(TagAt(index)); 63 } 64 65 66 bool 67 DiscoveryPacket::AddTag(uint16 type, const void *data, uint16 length, int32 index) 68 { 69 pppoe_tag *add = (pppoe_tag*) malloc(length + 4); 70 add->type = type; 71 add->length = length; 72 memcpy(add->data, data, length); 73 74 bool status; 75 if(index < 0) 76 status = fTags.AddItem(add); 77 else 78 status = fTags.AddItem(add, index); 79 if(!status) { 80 free(add); 81 return false; 82 } 83 84 return true; 85 } 86 87 88 bool 89 DiscoveryPacket::RemoveTag(pppoe_tag *tag) 90 { 91 if(!fTags.HasItem(tag)) 92 return false; 93 94 fTags.RemoveItem(tag); 95 free(tag); 96 97 return true; 98 } 99 100 101 pppoe_tag* 102 DiscoveryPacket::TagAt(int32 index) const 103 { 104 pppoe_tag *tag = fTags.ItemAt(index); 105 106 if(tag == fTags.GetDefaultItem()) 107 return NULL; 108 109 return tag; 110 } 111 112 113 pppoe_tag* 114 DiscoveryPacket::TagWithType(uint16 type) const 115 { 116 pppoe_tag *tag; 117 118 for(int32 index = 0; index < CountTags(); index++) { 119 tag = TagAt(index); 120 if(tag && tag->type == type) 121 return tag; 122 } 123 124 return NULL; 125 } 126 127 128 net_buffer* 129 DiscoveryPacket::ToNetBuffer(uint32 MTU) 130 { 131 net_buffer *packet = gBufferModule->create(256); 132 if (packet == NULL) { 133 dprintf("create buffer failure\n"); 134 return NULL; 135 } 136 137 pppoe_header *header ; 138 status_t status = gBufferModule->append_size(packet, 1492, (void **)&header); 139 if (status != B_OK) { 140 dprintf("append size failure\n"); 141 return NULL; 142 } 143 144 header->version = PPPoE_VERSION; 145 header->type = PPPoE_TYPE; 146 header->code = Code(); 147 header->sessionID = SessionID(); 148 149 uint16 length = 0; 150 pppoe_tag *tag; 151 152 for(int32 index = 0; index < CountTags(); index++) { 153 tag = TagAt(index); 154 155 // make sure we have enough space left 156 if(MTU - length < tag->length) { 157 gBufferModule->free(packet); 158 return NULL; 159 } 160 161 *((uint16*)(header->data + length)) = htons(tag->type); 162 length += 2; 163 *((uint16*)(header->data + length)) = htons(tag->length); 164 length += 2; 165 memcpy(header->data + length, tag->data, tag->length); 166 length += tag->length; 167 } 168 169 header->length = htons(length); 170 status = gBufferModule->trim(packet, length + PPPoE_HEADER_SIZE); 171 if (status != B_OK) { 172 dprintf("trim buffer failure\n"); 173 return NULL; 174 } 175 176 return packet; 177 } 178