|
Packit |
597cd4 |
/*******************************************************************************
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
LLDP Agent Daemon (LLDPAD) Software
|
|
Packit |
597cd4 |
Copyright(c) 2007-2010 Intel Corporation.
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
This program is free software; you can redistribute it and/or modify it
|
|
Packit |
597cd4 |
under the terms and conditions of the GNU General Public License,
|
|
Packit |
597cd4 |
version 2, as published by the Free Software Foundation.
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
This program is distributed in the hope it will be useful, but WITHOUT
|
|
Packit |
597cd4 |
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
Packit |
597cd4 |
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
|
|
Packit |
597cd4 |
more details.
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
You should have received a copy of the GNU General Public License along with
|
|
Packit |
597cd4 |
this program; if not, write to the Free Software Foundation, Inc.,
|
|
Packit |
597cd4 |
51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
The full GNU General Public License is included in this distribution in
|
|
Packit |
597cd4 |
the file called "COPYING".
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
Contact Information:
|
|
Packit |
597cd4 |
open-lldp Mailing List <lldp-devel@open-lldp.org>
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
*******************************************************************************/
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
#include <stdlib.h>
|
|
Packit |
597cd4 |
#include <string.h>
|
|
Packit |
597cd4 |
#include <arpa/inet.h>
|
|
Packit |
597cd4 |
#include "lldp_tlv.h"
|
|
Packit |
597cd4 |
#include "lldp.h"
|
|
Packit |
597cd4 |
#include "lldp/ports.h"
|
|
Packit |
597cd4 |
#include "lldp/l2_packet.h"
|
|
Packit |
597cd4 |
#include "dcb_types.h"
|
|
Packit |
597cd4 |
#include "messages.h"
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
void somethingChangedLocal(const char *ifname, int type)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct lldp_agent *agent;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
agent = lldp_agent_find_by_type(ifname, type);
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if (agent == NULL)
|
|
Packit |
597cd4 |
return;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
agent->tx.localChange = 1;
|
|
Packit |
597cd4 |
agent->tx.txFast = agent->timers.txFastInit;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
return;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
int tlv_ok(struct unpacked_tlv *tlv)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
if (!tlv || (!tlv->length && tlv->type))
|
|
Packit |
597cd4 |
return 0;
|
|
Packit |
597cd4 |
else
|
|
Packit |
597cd4 |
return 1;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct packed_tlv *pack_tlv(struct unpacked_tlv *tlv)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
u16 tl = 0;
|
|
Packit |
597cd4 |
struct packed_tlv *pkd_tlv = NULL;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if (!tlv_ok(tlv))
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
tl = tlv->type;
|
|
Packit |
597cd4 |
tl = tl << 9;
|
|
Packit |
597cd4 |
tl |= tlv->length & 0x01ff;
|
|
Packit |
597cd4 |
tl = htons(tl);
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
pkd_tlv = (struct packed_tlv *)malloc(sizeof(struct packed_tlv));
|
|
Packit |
597cd4 |
if(!pkd_tlv) {
|
|
Packit |
597cd4 |
LLDPAD_DBG("pack_tlv: Failed to malloc pkd_tlv\n");
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
memset(pkd_tlv,0,sizeof(struct packed_tlv));
|
|
Packit |
597cd4 |
pkd_tlv->size = tlv->length + sizeof(tl);
|
|
Packit |
597cd4 |
pkd_tlv->tlv = (u8 *)malloc(pkd_tlv->size);
|
|
Packit |
597cd4 |
if(pkd_tlv->tlv) {
|
|
Packit |
597cd4 |
memset(pkd_tlv->tlv,0, pkd_tlv->size);
|
|
Packit |
597cd4 |
memcpy(pkd_tlv->tlv, &tl, sizeof(tl));
|
|
Packit |
597cd4 |
if (tlv->length)
|
|
Packit |
597cd4 |
memcpy(&pkd_tlv->tlv[sizeof(tl)], tlv->info,
|
|
Packit |
597cd4 |
tlv->length);
|
|
Packit |
597cd4 |
} else {
|
|
Packit |
597cd4 |
LLDPAD_DBG("pack_tlv: Failed to malloc tlv\n");
|
|
Packit |
597cd4 |
free(pkd_tlv);
|
|
Packit |
597cd4 |
pkd_tlv = NULL;
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
return pkd_tlv;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
/*
|
|
Packit |
597cd4 |
* pack the input tlv and put it at the end of the already packed tlv mtlv
|
|
Packit |
597cd4 |
* update the total size of the out put mtlv
|
|
Packit |
597cd4 |
*/
|
|
Packit |
597cd4 |
int pack_tlv_after(struct unpacked_tlv *tlv, struct packed_tlv *mtlv, int length)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct packed_tlv *ptlv;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if (!tlv)
|
|
Packit |
597cd4 |
return 0; /* no TLV is ok */
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if (!tlv_ok(tlv))
|
|
Packit |
597cd4 |
return -1;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
ptlv = pack_tlv(tlv);
|
|
Packit |
597cd4 |
if (!ptlv)
|
|
Packit |
597cd4 |
return -1;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if (ptlv->size + mtlv->size > length) {
|
|
Packit |
597cd4 |
ptlv = free_pkd_tlv(ptlv);
|
|
Packit |
597cd4 |
return -1;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
memcpy(&mtlv->tlv[mtlv->size], ptlv->tlv, ptlv->size);
|
|
Packit |
597cd4 |
mtlv->size += ptlv->size;
|
|
Packit |
597cd4 |
ptlv = free_pkd_tlv(ptlv);
|
|
Packit |
597cd4 |
return 0;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct unpacked_tlv *unpack_tlv(struct packed_tlv *tlv)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
u16 tl = 0;
|
|
Packit |
597cd4 |
struct unpacked_tlv *upkd_tlv = NULL;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if(!tlv) {
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
memcpy(&tl,tlv->tlv, sizeof(tl));
|
|
Packit |
597cd4 |
tl = ntohs(tl);
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
upkd_tlv = (struct unpacked_tlv *)malloc(sizeof(struct unpacked_tlv));
|
|
Packit |
597cd4 |
if(upkd_tlv) {
|
|
Packit |
597cd4 |
memset(upkd_tlv,0, sizeof(struct unpacked_tlv));
|
|
Packit |
597cd4 |
upkd_tlv->length = tl & 0x01ff;
|
|
Packit |
597cd4 |
upkd_tlv->info = (u8 *)malloc(upkd_tlv->length);
|
|
Packit |
597cd4 |
if(upkd_tlv->info) {
|
|
Packit |
597cd4 |
memset(upkd_tlv->info,0, upkd_tlv->length);
|
|
Packit |
597cd4 |
tl = tl >> 9;
|
|
Packit |
597cd4 |
upkd_tlv->type = (u8)tl;
|
|
Packit |
597cd4 |
memcpy(upkd_tlv->info, &tlv->tlv[sizeof(tl)],
|
|
Packit |
597cd4 |
upkd_tlv->length);
|
|
Packit |
597cd4 |
} else {
|
|
Packit |
597cd4 |
LLDPAD_DBG("unpack_tlv: Failed to malloc info\n");
|
|
Packit |
597cd4 |
free (upkd_tlv);
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
} else {
|
|
Packit |
597cd4 |
LLDPAD_DBG("unpack_tlv: Failed to malloc upkd_tlv\n");
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
return upkd_tlv;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct unpacked_tlv *free_unpkd_tlv(struct unpacked_tlv *tlv)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
if (tlv != NULL) {
|
|
Packit |
597cd4 |
if (tlv->info != NULL) {
|
|
Packit |
597cd4 |
free(tlv->info);
|
|
Packit |
597cd4 |
tlv->info = NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
free(tlv);
|
|
Packit |
597cd4 |
tlv = NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct packed_tlv *free_pkd_tlv(struct packed_tlv *tlv)
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
if (tlv != NULL) {
|
|
Packit |
597cd4 |
if (tlv->tlv != NULL) {
|
|
Packit |
597cd4 |
free(tlv->tlv);
|
|
Packit |
597cd4 |
tlv->tlv = NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
free(tlv);
|
|
Packit |
597cd4 |
tlv = NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct packed_tlv *create_ptlv()
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct packed_tlv *ptlv =
|
|
Packit |
597cd4 |
(struct packed_tlv *)malloc(sizeof(struct packed_tlv));
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if(ptlv)
|
|
Packit |
597cd4 |
memset(ptlv, 0, sizeof(struct packed_tlv));
|
|
Packit |
597cd4 |
return ptlv;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct unpacked_tlv *create_tlv()
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct unpacked_tlv *tlv =
|
|
Packit |
597cd4 |
(struct unpacked_tlv *)malloc(sizeof(struct unpacked_tlv));
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if(tlv) {
|
|
Packit |
597cd4 |
memset(tlv,0, sizeof(struct unpacked_tlv));
|
|
Packit |
597cd4 |
return tlv;
|
|
Packit |
597cd4 |
} else {
|
|
Packit |
597cd4 |
LLDPAD_DBG("create_tlv: Failed to malloc tlv\n");
|
|
Packit |
597cd4 |
return NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct unpacked_tlv *bld_end_tlv()
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct unpacked_tlv *tlv = create_tlv();
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
if(tlv) {
|
|
Packit |
597cd4 |
tlv->type = END_OF_LLDPDU_TLV;
|
|
Packit |
597cd4 |
tlv->length = 0;
|
|
Packit |
597cd4 |
tlv->info = NULL;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
return tlv;
|
|
Packit |
597cd4 |
}
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
struct packed_tlv *pack_end_tlv()
|
|
Packit |
597cd4 |
{
|
|
Packit |
597cd4 |
struct unpacked_tlv *tlv;
|
|
Packit |
597cd4 |
struct packed_tlv *ptlv;
|
|
Packit |
597cd4 |
|
|
Packit |
597cd4 |
tlv = bld_end_tlv();
|
|
Packit |
597cd4 |
ptlv = pack_tlv(tlv);
|
|
Packit |
597cd4 |
free(tlv);
|
|
Packit |
597cd4 |
return ptlv;
|
|
Packit |
597cd4 |
}
|