Removed all old TLV code
This commit is contained in:
Gregg
parent
3c172da714
commit
6280c17154
144
src/HAP.cpp
144
src/HAP.cpp
|
|
@ -112,18 +112,6 @@ void HAPClient::init(){
|
|||
|
||||
printControllers();
|
||||
|
||||
// tlv8.create(kTLVType_Separator,0,"SEPARATOR"); // define the actual TLV records needed for the implementation of HAP; one for each kTLVType needed (HAP Table 5-6)
|
||||
// tlv8.create(kTLVType_State,1,"STATE");
|
||||
// tlv8.create(kTLVType_PublicKey,384,"PUBKEY");
|
||||
// tlv8.create(kTLVType_Method,1,"METHOD");
|
||||
// tlv8.create(kTLVType_Salt,16,"SALT");
|
||||
// tlv8.create(kTLVType_Error,1,"ERROR");
|
||||
// tlv8.create(kTLVType_Proof,64,"PROOF");
|
||||
// tlv8.create(kTLVType_EncryptedData,1024,"ENC.DATA");
|
||||
// tlv8.create(kTLVType_Signature,64,"SIGNATURE");
|
||||
// tlv8.create(kTLVType_Identifier,64,"IDENTIFIER");
|
||||
// tlv8.create(kTLVType_Permissions,1,"PERMISSION");
|
||||
|
||||
if(!nvs_get_blob(hapNVS,"HAPHASH",NULL,&len)){ // if found HAP HASH structure
|
||||
nvs_get_blob(hapNVS,"HAPHASH",&homeSpan.hapConfig,&len); // retrieve data
|
||||
} else {
|
||||
|
|
@ -782,42 +770,6 @@ int HAPClient::postPairVerifyURL(uint8_t *content, size_t len){
|
|||
|
||||
//////////////////////////////////////
|
||||
|
||||
int HAPClient::getAccessoriesURL(){
|
||||
|
||||
if(!cPair){ // unverified, unencrypted session
|
||||
unauthorizedError();
|
||||
return(0);
|
||||
}
|
||||
|
||||
LOG1("In Get Accessories #");
|
||||
LOG1(conNum);
|
||||
LOG1(" (");
|
||||
LOG1(client.remoteIP());
|
||||
LOG1(")...\n");
|
||||
|
||||
int nBytes = homeSpan.sprintfAttributes(NULL); // get size of HAP attributes JSON
|
||||
TempBuffer<char> jBuf(nBytes+1);
|
||||
homeSpan.sprintfAttributes(jBuf); // create JSON database (will need to re-cast to uint8_t* below)
|
||||
|
||||
char *body;
|
||||
asprintf(&body,"HTTP/1.1 200 OK\r\nContent-Type: application/hap+json\r\nContent-Length: %d\r\n\r\n",nBytes);
|
||||
|
||||
LOG2("\n>>>>>>>>>> ");
|
||||
LOG2(client.remoteIP());
|
||||
LOG2(" >>>>>>>>>>\n");
|
||||
LOG2(body);
|
||||
LOG2(jBuf.get());
|
||||
LOG2("\n");
|
||||
|
||||
sendEncrypted(body,(uint8_t *)jBuf.get(),nBytes);
|
||||
free(body);
|
||||
|
||||
return(1);
|
||||
|
||||
} // getAccessories
|
||||
|
||||
//////////////////////////////////////
|
||||
|
||||
int HAPClient::postPairingsURL(uint8_t *content, size_t len){
|
||||
|
||||
if(!cPair){ // unverified, unencrypted session
|
||||
|
|
@ -951,8 +903,43 @@ int HAPClient::postPairingsURL(uint8_t *content, size_t len){
|
|||
|
||||
//////////////////////////////////////
|
||||
|
||||
int HAPClient::getCharacteristicsURL(char *urlBuf){
|
||||
int HAPClient::getAccessoriesURL(){
|
||||
|
||||
if(!cPair){ // unverified, unencrypted session
|
||||
unauthorizedError();
|
||||
return(0);
|
||||
}
|
||||
|
||||
LOG1("In Get Accessories #");
|
||||
LOG1(conNum);
|
||||
LOG1(" (");
|
||||
LOG1(client.remoteIP());
|
||||
LOG1(")...\n");
|
||||
|
||||
int nBytes = homeSpan.sprintfAttributes(NULL); // get size of HAP attributes JSON
|
||||
TempBuffer<char> jBuf(nBytes+1);
|
||||
homeSpan.sprintfAttributes(jBuf); // create JSON database (will need to re-cast to uint8_t* below)
|
||||
|
||||
char *body;
|
||||
asprintf(&body,"HTTP/1.1 200 OK\r\nContent-Type: application/hap+json\r\nContent-Length: %d\r\n\r\n",nBytes);
|
||||
|
||||
LOG2("\n>>>>>>>>>> ");
|
||||
LOG2(client.remoteIP());
|
||||
LOG2(" >>>>>>>>>>\n");
|
||||
LOG2(body);
|
||||
LOG2(jBuf.get());
|
||||
LOG2("\n");
|
||||
|
||||
sendEncrypted(body,(uint8_t *)jBuf.get(),nBytes);
|
||||
free(body);
|
||||
|
||||
return(1);
|
||||
|
||||
} // getAccessories
|
||||
|
||||
//////////////////////////////////////
|
||||
|
||||
int HAPClient::getCharacteristicsURL(char *urlBuf){
|
||||
|
||||
if(!cPair){ // unverified, unencrypted session
|
||||
unauthorizedError();
|
||||
|
|
@ -1390,34 +1377,6 @@ void HAPClient::tlvRespond(TLV8 &tlv8){
|
|||
|
||||
//////////////////////////////////////
|
||||
|
||||
//void HAPClient::tlvRespond(){
|
||||
//
|
||||
// TempBuffer<uint8_t> tBuf(tlv8.pack(NULL)); // create buffer to hold TLV data
|
||||
// tlv8.pack(tBuf); // pack TLV records into buffer
|
||||
//
|
||||
// char *body;
|
||||
// asprintf(&body,"HTTP/1.1 200 OK\r\nContent-Type: application/pairing+tlv8\r\nContent-Length: %d\r\n\r\n",tBuf.len()); // create Body with Content Length = size of TLV data
|
||||
//
|
||||
// LOG2("\n>>>>>>>>>> ");
|
||||
// LOG2(client.remoteIP());
|
||||
// LOG2(" >>>>>>>>>>\n");
|
||||
// LOG2(body);
|
||||
// tlv8.print(2);
|
||||
//
|
||||
// if(!cPair){ // unverified, unencrypted session
|
||||
// client.print(body);
|
||||
// client.write(tBuf,tBuf.len());
|
||||
// LOG2("------------ SENT! --------------\n");
|
||||
// } else {
|
||||
// sendEncrypted(body,tBuf,tBuf.len());
|
||||
// }
|
||||
//
|
||||
// free(body);
|
||||
//
|
||||
//} // tlvRespond
|
||||
|
||||
//////////////////////////////////////
|
||||
|
||||
int HAPClient::receiveEncrypted(uint8_t *httpBuf, int messageSize){
|
||||
|
||||
uint8_t aad[2];
|
||||
|
|
@ -1637,36 +1596,6 @@ void HAPClient::tearDown(uint8_t *id){
|
|||
|
||||
//////////////////////////////////////
|
||||
|
||||
//int HAPClient::listControllers(uint8_t *tlvBuf){
|
||||
//
|
||||
// int nBytes=0;
|
||||
// int n;
|
||||
//
|
||||
// tlv8.clear();
|
||||
// tlv8.val(kTLVType_State,pairState_M2);
|
||||
//
|
||||
// for(auto it=controllerList.begin();it!=controllerList.end();it++){
|
||||
// if((*it).allocated){
|
||||
// if(tlv8.val(kTLVType_State)==-1) // if State is not set then this is not the first controller found
|
||||
// tlv8.val(kTLVType_Separator,1);
|
||||
// tlv8.val(kTLVType_Permissions,(*it).admin);
|
||||
// tlv8.buf(kTLVType_Identifier,(*it).ID,36);
|
||||
// tlv8.buf(kTLVType_PublicKey,(*it).LTPK,32);
|
||||
// n=tlv8.pack(tlvBuf);
|
||||
// nBytes+=n;
|
||||
// if(tlvBuf){
|
||||
// tlvBuf+=n;
|
||||
// tlv8.print();
|
||||
// }
|
||||
// tlv8.clear();
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// return(nBytes);
|
||||
//}
|
||||
|
||||
//////////////////////////////////////
|
||||
|
||||
void HAPClient::printControllers(int minLogLevel){
|
||||
|
||||
if(homeSpan.logLevel<minLogLevel)
|
||||
|
|
@ -1734,7 +1663,6 @@ void Nonce::inc(){
|
|||
|
||||
// instantiate all static HAP Client structures and data
|
||||
|
||||
//TLV<kTLVType,11> HAPClient::tlv8;
|
||||
nvs_handle HAPClient::hapNVS;
|
||||
nvs_handle HAPClient::srpNVS;
|
||||
HKDF HAPClient::hkdf;
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
#include <WiFi.h>
|
||||
|
||||
#include "HomeSpan.h"
|
||||
//#include "TLV.h"
|
||||
#include "HAPConstants.h"
|
||||
#include "HKDF.h"
|
||||
#include "SRP.h"
|
||||
|
|
@ -105,7 +104,6 @@ struct HAPClient {
|
|||
static const int MAX_CONTROLLERS=16; // maximum number of paired controllers (HAP requires at least 16)
|
||||
static const int MAX_ACCESSORIES=150; // maximum number of allowed Accessories (HAP limit=150)
|
||||
|
||||
// static TLV<kTLVType,11> tlv8; // TLV8 structure (HAP Section 14.1) with space for 11 TLV records of type kTLVType (HAP Table 5-6)
|
||||
static nvs_handle hapNVS; // handle for non-volatile-storage of HAP data
|
||||
static nvs_handle srpNVS; // handle for non-volatile-storage of SRP data
|
||||
static HKDF hkdf; // generates (and stores) HKDF-SHA-512 32-byte keys derived from an inputKey of arbitrary length, a salt string, and an info string
|
||||
|
|
@ -146,7 +144,6 @@ struct HAPClient {
|
|||
int putPrepareURL(char *json); // PUT /prepare (HAP Section 6.7.2.4)
|
||||
int getStatusURL(); // GET / status (an optional, non-HAP feature)
|
||||
|
||||
// void tlvRespond(); // respond to client with HTTP OK header and all defined TLV data records (those with length>0)
|
||||
void tlvRespond(TLV8 &tlv8); // respond to client with HTTP OK header and all defined TLV data records
|
||||
void sendEncrypted(char *body, uint8_t *dataBuf, int dataLen); // send client complete ChaCha20-Poly1305 encrypted HTTP mesage comprising a null-terminated 'body' and 'dataBuf' with 'dataLen' bytes
|
||||
int receiveEncrypted(uint8_t *httpBuf, int messageSize); // decrypt HTTP request (HAP Section 6.5)
|
||||
|
|
@ -167,7 +164,6 @@ struct HAPClient {
|
|||
static tagError addController(uint8_t *id, uint8_t *ltpk, boolean admin); // stores data for new Controller with specified data. Returns tagError (if any)
|
||||
static void removeController(uint8_t *id); // removes specific Controller. If no remaining admin Controllers, remove all others (if any) as per HAP requirements.
|
||||
static void printControllers(int minLogLevel=0); // prints IDs of all allocated (paired) Controller, subject to specified minimum log level
|
||||
// static int listControllers(uint8_t *tlvBuf); // creates and prints a multi-TLV list of Controllers (HAP Section 5.12)
|
||||
static void saveControllers(); // saves Controller list in NVS
|
||||
static int nAdminControllers(); // returns number of admin Controller
|
||||
static void tearDown(uint8_t *id); // tears down connections using Controller with ID=id; tears down all connections if id=NULL
|
||||
|
|
|
|||
27
src/SRP.cpp
27
src/SRP.cpp
|
|
@ -239,33 +239,6 @@ void SRP6A::createProof(){
|
|||
|
||||
//////////////////////////////////////
|
||||
|
||||
//int SRP6A::loadTLV(kTLVType tag, mbedtls_mpi *mpi, int nBytes){
|
||||
//
|
||||
// uint8_t *buf=HAPClient::tlv8.buf(tag,nBytes);
|
||||
//
|
||||
// if(!buf)
|
||||
// return(0);
|
||||
//
|
||||
// mbedtls_mpi_write_binary(mpi,buf,nBytes);
|
||||
// return(1);
|
||||
//}
|
||||
//
|
||||
////////////////////////////////////////
|
||||
//
|
||||
//int SRP6A::writeTLV(kTLVType tag, mbedtls_mpi *mpi){
|
||||
//
|
||||
// int nBytes=HAPClient::tlv8.len(tag);
|
||||
//
|
||||
// if(nBytes>0){
|
||||
// mbedtls_mpi_read_binary(mpi,HAPClient::tlv8.buf(tag),nBytes);
|
||||
// return(1);
|
||||
// };
|
||||
//
|
||||
// return(0);
|
||||
//}
|
||||
|
||||
//////////////////////////////////////
|
||||
|
||||
void SRP6A::print(mbedtls_mpi *mpi, int minLogLevel){
|
||||
|
||||
if(homeSpan.getLogLevel()<minLogLevel)
|
||||
|
|
|
|||
|
|
@ -79,11 +79,7 @@ struct SRP6A {
|
|||
void getPrivateKey(); // generates and stores random 32-byte private key, b
|
||||
void getSetupCode(char *c); // generates and displays random 8-digit Pair-Setup code, P, in format XXX-XX-XXX
|
||||
void createPublicKey(); // computes x, v, and B from random s, P, and b
|
||||
void createSessionKey(); // computes u from A and B, and then S from A, v, u, and b
|
||||
|
||||
// int loadTLV(kTLVType tag, mbedtls_mpi *mpi, int nBytes); // load binary contents of mpi into a TLV record and set its length
|
||||
// int writeTLV(kTLVType tag, mbedtls_mpi *mpi); // write binary contents of a TLV record into an mpi
|
||||
|
||||
void createSessionKey(); // computes u from A and B, and then S from A, v, u, and b
|
||||
int verifyProof(); // verify M1 SRP6A Proof received from HAP client (return 1 on success, 0 on failure)
|
||||
void createProof(); // create M2 server-side SRP6A Proof based on M1 as received from HAP Client
|
||||
|
||||
|
|
|
|||
335
src/TLV.h
335
src/TLV.h
|
|
@ -1,335 +0,0 @@
|
|||
/*********************************************************************************
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2020-2023 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.
|
||||
*
|
||||
********************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
template <class tagType, int maxTags>
|
||||
class TLV {
|
||||
|
||||
int cLen; // total number of bytes in all defined TLV records, including TAG and LEN (suitable for use as Content-Length in HTTP Body)
|
||||
int numTags; // actual number of tags defined
|
||||
|
||||
struct tlv_t {
|
||||
tagType tag; // TAG
|
||||
int len; // LENGTH
|
||||
uint8_t *val; // VALUE buffer
|
||||
int maxLen; // maximum length of VALUE buffer
|
||||
const char *name; // abbreviated name of this TAG
|
||||
};
|
||||
|
||||
tlv_t tlv[maxTags]; // pointer to array of TLV record structures
|
||||
tlv_t *find(tagType tag); // returns pointer to TLV record with matching TAG (or NULL if no match)
|
||||
|
||||
public:
|
||||
|
||||
TLV();
|
||||
|
||||
int create(tagType tag, int maxLen, const char *name); // creates a new TLV record of type 'tag' with 'maxLen' bytes and display 'name'
|
||||
|
||||
void clear(); // clear all TLV structures
|
||||
int val(tagType tag); // returns VAL for TLV with matching TAG (or -1 if no match)
|
||||
int val(tagType tag, uint8_t val); // sets and returns VAL for TLV with matching TAG (or -1 if no match)
|
||||
uint8_t *buf(tagType tag); // returns VAL Buffer for TLV with matching TAG (or NULL if no match)
|
||||
uint8_t *buf(tagType tag, int len); // set length and returns VAL Buffer for TLV with matching TAG (or NULL if no match or if LEN>MAX)
|
||||
uint8_t *buf(tagType tag, uint8_t *src, int len); // copies len bytes of src into VAL buffer, and sets length to len, for TLV with matching TAG; returns VAL Buffer on success, or NULL if no match or if LEN>MAX
|
||||
int len(tagType tag); // returns LEN for TLV matching TAG (or 0 if TAG is found but LEN not yet set; -1 if no match at all)
|
||||
void print(int minLogLevel=0); // prints all defined TLVs (those with length>0), subject to specified minimum log level
|
||||
int unpack(uint8_t *tlvBuf, int nBytes); // unpacks nBytes of TLV content from single byte buffer into individual TLV records (return 1 on success, 0 if fail)
|
||||
int pack(uint8_t *tlvBuf); // if tlvBuf!=NULL, packs all defined TLV records (LEN>0) into a single byte buffer, spitting large TLVs into separate 255-byte chunks. Returns number of bytes (that would be) stored in buffer
|
||||
|
||||
}; // TLV
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV contructor()
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
TLV<tagType, maxTags>::TLV(){
|
||||
numTags=0;
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV create(tag, maxLen, name)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::create(tagType tag, int maxLen, const char *name){
|
||||
|
||||
if(numTags==maxTags){
|
||||
Serial.print("\n*** ERROR: Can't create new TLC tag type with name='");
|
||||
Serial.print(name);
|
||||
Serial.print("' - exceeded number of records reserved\n\n");
|
||||
return(0);
|
||||
}
|
||||
|
||||
tlv[numTags].tag=tag;
|
||||
tlv[numTags].maxLen=maxLen;
|
||||
tlv[numTags].name=name;
|
||||
tlv[numTags].len=-1;
|
||||
tlv[numTags].val=(uint8_t *)HS_MALLOC(maxLen);
|
||||
numTags++;
|
||||
|
||||
return(1);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV find(tag)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
typename TLV<tagType, maxTags>::tlv_t *TLV<tagType, maxTags>::find(tagType tag){
|
||||
|
||||
for(int i=0;i<numTags;i++){
|
||||
if(tlv[i].tag==tag)
|
||||
return(tlv+i);
|
||||
}
|
||||
|
||||
return(NULL);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV clear()
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
void TLV<tagType, maxTags>::clear(){
|
||||
|
||||
cLen=0;
|
||||
|
||||
for(int i=0;i<numTags;i++)
|
||||
tlv[i].len=-1;
|
||||
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV val(tag)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::val(tagType tag){
|
||||
|
||||
tlv_t *tlv=find(tag);
|
||||
|
||||
if(tlv && tlv->len>=0){
|
||||
if(tlv->maxLen>0)
|
||||
return(tlv->val[0]);
|
||||
else
|
||||
return(0);
|
||||
}
|
||||
|
||||
return(-1);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV val(tag, val)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::val(tagType tag, uint8_t val){
|
||||
|
||||
tlv_t *tlv=find(tag);
|
||||
|
||||
if(tlv){
|
||||
if(tlv->maxLen>0)
|
||||
tlv->val[0]=val;
|
||||
tlv->len=(tlv->maxLen>0);
|
||||
cLen+=tlv->len+2;
|
||||
return(val);
|
||||
}
|
||||
|
||||
return(-1);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV buf(tag)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
uint8_t *TLV<tagType, maxTags>::buf(tagType tag){
|
||||
|
||||
tlv_t *tlv=find(tag);
|
||||
|
||||
if(tlv)
|
||||
return(tlv->val);
|
||||
|
||||
return(NULL);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV buf(tag, len)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
uint8_t *TLV<tagType, maxTags>::buf(tagType tag, int len){
|
||||
|
||||
return(buf(tag,NULL,len));
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV buf(tag, src, len)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
uint8_t *TLV<tagType, maxTags>::buf(tagType tag, uint8_t *src, int len){
|
||||
|
||||
tlv_t *tlv=find(tag);
|
||||
|
||||
if(tlv && len<=tlv->maxLen){
|
||||
tlv->len=len;
|
||||
cLen+=tlv->len;
|
||||
|
||||
for(int i=0;i<tlv->len;i+=255)
|
||||
cLen+=2;
|
||||
|
||||
if(src)
|
||||
memcpy(tlv->val,src,len);
|
||||
|
||||
return(tlv->val);
|
||||
}
|
||||
|
||||
return(NULL);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV print()
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
void TLV<tagType, maxTags>::print(int minLogLevel){
|
||||
|
||||
if(homeSpan.getLogLevel()<minLogLevel)
|
||||
return;
|
||||
|
||||
for(int i=0;i<numTags;i++){
|
||||
|
||||
if(tlv[i].len>=0){
|
||||
Serial.printf("%s(%d) ",tlv[i].name,tlv[i].len);
|
||||
|
||||
for(int j=0;j<tlv[i].len;j++)
|
||||
Serial.printf("%02X",tlv[i].val[j]);
|
||||
|
||||
Serial.printf("\n");
|
||||
|
||||
} // len>0
|
||||
} // loop over all TLVs
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV pack(tlvBuf)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::pack(uint8_t *tlvBuf){
|
||||
|
||||
int n=0;
|
||||
int nBytes;
|
||||
|
||||
for(int i=0;i<numTags;i++){
|
||||
|
||||
if((nBytes=tlv[i].len)>=0){
|
||||
int j=0;
|
||||
do{
|
||||
int wBytes=nBytes>255?255:nBytes;
|
||||
if(tlvBuf!=NULL){
|
||||
*tlvBuf++=tlv[i].tag;
|
||||
*tlvBuf++=wBytes;
|
||||
memcpy(tlvBuf,tlv[i].val+j,wBytes);
|
||||
tlvBuf+=wBytes;
|
||||
}
|
||||
n+=wBytes+2;
|
||||
j+=wBytes;
|
||||
nBytes-=wBytes;
|
||||
} while(nBytes>0);
|
||||
} // len>=0
|
||||
|
||||
} // loop over all TLVs
|
||||
|
||||
return(n);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV len(tag)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::len(tagType tag){
|
||||
|
||||
tlv_t *tlv=find(tag);
|
||||
|
||||
if(tlv)
|
||||
return(tlv->len>0?tlv->len:0);
|
||||
|
||||
return(-1);
|
||||
}
|
||||
|
||||
//////////////////////////////////////
|
||||
// TLV unpack(tlvBuf, nBytes)
|
||||
|
||||
template<class tagType, int maxTags>
|
||||
int TLV<tagType, maxTags>::unpack(uint8_t *tlvBuf, int nBytes){
|
||||
|
||||
clear();
|
||||
|
||||
tagType tag;
|
||||
int tagLen;
|
||||
uint8_t *val;
|
||||
int currentLen;
|
||||
int state=0;
|
||||
|
||||
for(int i=0;i<nBytes;i++){
|
||||
|
||||
switch(state){
|
||||
|
||||
case 0: // ready to read next tag
|
||||
if((tag=(tagType)tlvBuf[i])==-1){ // read TAG; return with error if not found
|
||||
clear();
|
||||
return(0);
|
||||
}
|
||||
state=1;
|
||||
break;
|
||||
|
||||
case 1: // ready to read tag length
|
||||
tagLen=tlvBuf[i]; // read LEN
|
||||
currentLen=len(tag); // get current length of existing tag
|
||||
if(!(val=buf(tag,tagLen+currentLen))){ // get VAL Buffer for TAG and set LEN (returns NULL if LEN > maxLen)
|
||||
clear();
|
||||
return(0);
|
||||
}
|
||||
|
||||
val+=currentLen; // move val to end of current length (tag repeats to load more than 255 bytes)
|
||||
|
||||
if(tagLen==0) // no bytes to read
|
||||
state=0;
|
||||
else // move to next state
|
||||
state=2;
|
||||
break;
|
||||
|
||||
case 2: // ready to read another byte into VAL
|
||||
*val=tlvBuf[i]; // copy byte into VAL buffer
|
||||
val++; // increment VAL buffer (already checked for sufficient length above)
|
||||
tagLen--; // decrement number of bytes to continue copying
|
||||
if(tagLen==0) // no more bytes to copy
|
||||
state=0;
|
||||
break;
|
||||
|
||||
} // switch
|
||||
} // for-loop
|
||||
|
||||
if(state==0) // should always end back in state=0
|
||||
return(1); // return success
|
||||
|
||||
clear();
|
||||
return(0); // return fail
|
||||
}
|
||||
Loading…
Reference in New Issue