src/add-ons/kernel/network/ppp/pppoe/DiscoveryPacket.cpp

   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 }