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 "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 }