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