300 lines
7.5 KiB
C++
300 lines
7.5 KiB
C++
/*********************************************************************************
|
|
* MIT License
|
|
*
|
|
* Copyright (c) 2020-2024 Gregg E. Berman
|
|
*
|
|
* https://github.com/HomeSpan/HomeSpan
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*
|
|
********************************************************************************/
|
|
|
|
#include "TLV8.h"
|
|
|
|
//////////////////////////////////////
|
|
|
|
tlv8_t::tlv8_t(uint8_t tag, size_t len, const uint8_t* val) : tag{tag}, len{len} {
|
|
if(len>0){
|
|
this->val=std::unique_ptr<uint8_t>((uint8_t *)HS_MALLOC(len));
|
|
if(val!=NULL)
|
|
memcpy((this->val).get(),val,len);
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
|
|
void tlv8_t::update(size_t addLen, const uint8_t *addVal){
|
|
if(addLen>0){
|
|
uint8_t *p=val.release();
|
|
p=(uint8_t *)HS_REALLOC(p,len+addLen);
|
|
val=std::unique_ptr<uint8_t>(p);
|
|
if(addVal!=NULL)
|
|
memcpy(p+len,addVal,addLen);
|
|
len+=addLen;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
void tlv8_t::osprint(std::ostream& os){
|
|
|
|
uint8_t *p=val.get(); // starting pointer
|
|
uint8_t *pend=p+len; // ending pointer (may equal starting if len=0)
|
|
|
|
do{
|
|
uint8_t nBytes=(pend-p)>255?255:(pend-p); // max is 255 bytes per TLV record
|
|
os.write((char *)&tag,1);
|
|
os.write((char *)&nBytes,1);
|
|
os.write((char *)p,nBytes);
|
|
p+=nBytes;
|
|
} while(p<pend);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
TLV8_it TLV8::add(uint8_t tag, size_t len, const uint8_t* val){
|
|
|
|
if(!empty() && back().getTag()==tag)
|
|
back().update(len,val);
|
|
else
|
|
emplace_back(tag,len,val);
|
|
|
|
return(--end());
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
TLV8_it TLV8::add(uint8_t tag, TLV8 &subTLV){
|
|
|
|
auto it=add(tag,subTLV.pack_size(),NULL); // create space for inserting sub TLV and store iterator to new element
|
|
subTLV.pack(*it); // pack subTLV into new element
|
|
return(--end());
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
TLV8_it TLV8::add(uint8_t tag, uint64_t val){
|
|
|
|
uint8_t *p=reinterpret_cast<uint8_t *>(&val);
|
|
size_t nBytes=sizeof(uint64_t);
|
|
while(nBytes>1 && p[nBytes-1]==0) // TLV requires little endian of size 1, 2, 4, or 8 bytes (include trailing zeros as needed)
|
|
nBytes--;
|
|
if(nBytes==3) // need to include a trailing zero so that total bytes=4
|
|
nBytes=4;
|
|
else if(nBytes>4) // need to include multiple trailing zeros so that total bytes=8
|
|
nBytes=8;
|
|
return(add(tag, nBytes, p));
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
TLV8_it TLV8::find(uint8_t tag, TLV8_it it1, TLV8_it it2){
|
|
|
|
auto it=it1;
|
|
while(it!=it2 && it->getTag()!=tag)
|
|
it++;
|
|
return(it);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
size_t TLV8::pack_size(TLV8_it it1, TLV8_it it2){
|
|
|
|
size_t nBytes=0;
|
|
|
|
while(it1!=it2){
|
|
nBytes+=2+(*it1).getLen();
|
|
if((*it1).getLen()>255)
|
|
nBytes+=2*(((*it1).getLen()-1)/255);
|
|
it1++;
|
|
}
|
|
|
|
return(nBytes);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
size_t TLV8::pack(uint8_t *buf, size_t bufSize){
|
|
|
|
size_t nBytes=0;
|
|
|
|
while(nBytes<bufSize && currentPackIt!=endPackIt){
|
|
switch(currentPackPhase){
|
|
|
|
case 0:
|
|
currentPackBuf=*currentPackIt;
|
|
endPackBuf=(*currentPackIt)+currentPackIt->getLen();
|
|
currentPackPhase=1;
|
|
break;
|
|
|
|
case 1:
|
|
*buf++=currentPackIt->getTag();
|
|
nBytes++;
|
|
currentPackPhase=2;
|
|
break;
|
|
|
|
case 2:
|
|
currentPackLen=endPackBuf-currentPackBuf;
|
|
if(currentPackLen>255)
|
|
currentPackLen=255;
|
|
*buf++=currentPackLen;
|
|
nBytes++;
|
|
currentPackPhase=3;
|
|
break;
|
|
|
|
case 3:
|
|
if(currentPackLen==0){
|
|
if(endPackBuf==currentPackBuf){
|
|
currentPackIt++;
|
|
currentPackPhase=0;
|
|
} else {
|
|
currentPackPhase=1;
|
|
}
|
|
break;
|
|
}
|
|
|
|
size_t copyBytes=(currentPackLen<(bufSize-nBytes)) ? currentPackLen : (bufSize-nBytes);
|
|
memcpy(buf,currentPackBuf,copyBytes);
|
|
buf+=copyBytes;
|
|
currentPackBuf+=copyBytes;
|
|
currentPackLen-=copyBytes;
|
|
nBytes+=copyBytes;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return(nBytes);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
int TLV8::unpack(uint8_t *buf, size_t bufSize){
|
|
|
|
if(bufSize==0)
|
|
return(-1);
|
|
|
|
if(empty())
|
|
unpackPhase=0;
|
|
|
|
while(bufSize>0){
|
|
switch(unpackPhase){
|
|
|
|
case 0:
|
|
unpackTag=*buf++;
|
|
bufSize--;
|
|
add(unpackTag);
|
|
unpackPhase=1;
|
|
break;
|
|
|
|
case 1:
|
|
unpackBytes=*buf++;
|
|
bufSize--;
|
|
if(unpackBytes==0)
|
|
unpackPhase=0;
|
|
else
|
|
unpackPhase=2;
|
|
break;
|
|
|
|
case 2:
|
|
size_t copyBytes=unpackBytes<bufSize ? unpackBytes : bufSize;
|
|
add(unpackTag,copyBytes,buf);
|
|
buf+=copyBytes;
|
|
unpackBytes-=copyBytes;
|
|
bufSize-=copyBytes;
|
|
if(unpackBytes==0)
|
|
unpackPhase=0;
|
|
break;
|
|
}
|
|
}
|
|
return(unpackPhase);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
int TLV8::unpack(TLV8_it it){
|
|
|
|
if(it==end())
|
|
return(0);
|
|
|
|
return(unpack(*it,it->getLen()));
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
const char *TLV8::getName(uint8_t tag){
|
|
|
|
if(names==NULL)
|
|
return(NULL);
|
|
|
|
for(int i=0;i<nNames;i++){
|
|
if(names[i].tag==tag)
|
|
return(names[i].name);
|
|
}
|
|
|
|
return(NULL);
|
|
}
|
|
|
|
/////////////////////////////////////
|
|
|
|
void TLV8::print(TLV8_it it1, TLV8_it it2){
|
|
|
|
while(it1!=it2){
|
|
const char *name=getName(it1->getTag());
|
|
if(name)
|
|
Serial.printf("%s",name);
|
|
else
|
|
Serial.printf("%d",it1->getTag());
|
|
Serial.printf("(%d) ",it1->getLen());
|
|
for(int i=0;i<it1->getLen();i++)
|
|
Serial.printf("%02X",(*it1)[i]);
|
|
if(it1->getLen()==0)
|
|
Serial.printf(" [null]");
|
|
else if(it1->getLen()<=4)
|
|
Serial.printf(" [%u]",it1->getVal());
|
|
else if(it1->getLen()<=8)
|
|
Serial.printf(" [%llu]",it1->getVal<uint64_t>());
|
|
Serial.printf("\n");
|
|
it1++;
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
|
|
void TLV8::printAll_r(String label){
|
|
|
|
for(auto it=begin();it!=end();it++){
|
|
Serial.printf("%s",label.c_str());
|
|
print(it);
|
|
TLV8 tlv;
|
|
if(tlv.unpack(*it,(*it).getLen())==0)
|
|
tlv.printAll_r(label+String((*it).getTag())+"-");
|
|
}
|
|
Serial.printf("%sDONE\n",label.c_str());
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
|
|
void TLV8::osprint(std::ostream& os, TLV8_it it1, TLV8_it it2){
|
|
|
|
for(auto it=it1;it!=it2;it++)
|
|
(*it).osprint(os);
|
|
}
|
|
|
|
//////////////////////////////////////
|