src/lib/core/CHIPTLVWriter.cpp

   1 /*
   2  *
   3  *    Copyright (c) 2020-2021 Project CHIP Authors
   4  *    Copyright (c) 2013-2017 Nest Labs, Inc.
   5  *
   6  *    Licensed under the Apache License, Version 2.0 (the "License");
   7  *    you may not use this file except in compliance with the License.
   8  *    You may obtain a copy of the License at
   9  *
  10  *        http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  *    Unless required by applicable law or agreed to in writing, software
  13  *    distributed under the License is distributed on an "AS IS" BASIS,
  14  *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  *    See the License for the specific language governing permissions and
  16  *    limitations under the License.
  17  */
  18
  19 /**
  20  *    @file
  21  *      This file implements an encoder for the CHIP TLV (Tag-Length-Value) encoding format.
  22  *
  23  */
  24
  25 #ifndef __STDC_LIMIT_MACROS
  26 #define __STDC_LIMIT_MACROS
  27 #endif
  28 #include <core/CHIPTLV.h>
  29
  30 #include <core/CHIPCore.h>
  31 #include <core/CHIPEncoding.h>
  32
  33 #include <support/CHIPMem.h>
  34 #include <support/CodeUtils.h>
  35 #include <support/SafeInt.h>
  36
  37 #include <stdarg.h>
  38 #include <stdint.h>
  39 #include <string.h>
  40
  41 // Doxygen is confused by the __attribute__ annotation
  42 #ifndef DOXYGEN
  43 #define NO_INLINE __attribute__((noinline))
  44 #endif // DOXYGEN
  45
  46 namespace chip {
  47 namespace TLV {
  48
  49 using namespace chip::Encoding;
  50
  51 NO_INLINE void TLVWriter::Init(uint8_t * buf, uint32_t maxLen)
  52 {
  53     mBackingStore = nullptr;
  54     mBufStart = mWritePoint = buf;
  55     mRemainingLen           = maxLen;
  56     mLenWritten             = 0;
  57     mMaxLen                 = maxLen;
  58     mContainerType          = kTLVType_NotSpecified;
  59     SetContainerOpen(false);
  60     SetCloseContainerReserved(true);
  61
  62     ImplicitProfileId = kProfileIdNotSpecified;
  63 }
  64
  65 CHIP_ERROR TLVWriter::Init(TLVBackingStore & backingStore, uint32_t maxLen)
  66 {
  67     mBackingStore  = &backingStore;
  68     mBufStart      = nullptr;
  69     mRemainingLen  = 0;
  70     CHIP_ERROR err = mBackingStore->OnInit(*this, mBufStart, mRemainingLen);
  71     if (err != CHIP_NO_ERROR)
  72         return err;
  73
  74     mWritePoint    = mBufStart;
  75     mLenWritten    = 0;
  76     mMaxLen        = maxLen;
  77     mContainerType = kTLVType_NotSpecified;
  78     SetContainerOpen(false);
  79     SetCloseContainerReserved(true);
  80
  81     ImplicitProfileId = kProfileIdNotSpecified;
  82     return CHIP_NO_ERROR;
  83 }
  84
  85 CHIP_ERROR TLVWriter::Finalize()
  86 {
  87     CHIP_ERROR err = CHIP_NO_ERROR;
  88     if (IsContainerOpen())
  89         return CHIP_ERROR_TLV_CONTAINER_OPEN;
  90     if (mBackingStore != nullptr)
  91         err = mBackingStore->FinalizeBuffer(*this, mBufStart, static_cast<uint32_t>(mWritePoint - mBufStart));
  92     return err;
  93 }
  94
  95 CHIP_ERROR TLVWriter::PutBoolean(uint64_t tag, bool v)
  96 {
  97     return WriteElementHead((v) ? TLVElementType::BooleanTrue : TLVElementType::BooleanFalse, tag, 0);
  98 }
  99
 100 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint8_t v)
 101 {
 102     return Put(tag, static_cast<uint64_t>(v));
 103 }
 104
 105 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint8_t v, bool preserveSize)
 106 {
 107     if (preserveSize)
 108         return WriteElementHead(TLVElementType::UInt8, tag, v);
 109     return Put(tag, v);
 110 }
 111
 112 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint16_t v)
 113 {
 114     return Put(tag, static_cast<uint64_t>(v));
 115 }
 116
 117 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint16_t v, bool preserveSize)
 118 {
 119     if (preserveSize)
 120         return WriteElementHead(TLVElementType::UInt16, tag, v);
 121     return Put(tag, v);
 122 }
 123
 124 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint32_t v)
 125 {
 126     return Put(tag, static_cast<uint64_t>(v));
 127 }
 128
 129 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint32_t v, bool preserveSize)
 130 {
 131     if (preserveSize)
 132         return WriteElementHead(TLVElementType::UInt32, tag, v);
 133     return Put(tag, v);
 134 }
 135
 136 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint64_t v)
 137 {
 138     TLVElementType elemType;
 139     if (v <= UINT8_MAX)
 140         elemType = TLVElementType::UInt8;
 141     else if (v <= UINT16_MAX)
 142         elemType = TLVElementType::UInt16;
 143     else if (v <= UINT32_MAX)
 144         elemType = TLVElementType::UInt32;
 145     else
 146         elemType = TLVElementType::UInt64;
 147     return WriteElementHead(elemType, tag, v);
 148 }
 149
 150 CHIP_ERROR TLVWriter::Put(uint64_t tag, uint64_t v, bool preserveSize)
 151 {
 152     if (preserveSize)
 153         return WriteElementHead(TLVElementType::UInt64, tag, v);
 154     return Put(tag, v);
 155 }
 156
 157 CHIP_ERROR TLVWriter::Put(uint64_t tag, int8_t v)
 158 {
 159     return Put(tag, static_cast<int64_t>(v));
 160 }
 161
 162 CHIP_ERROR TLVWriter::Put(uint64_t tag, int8_t v, bool preserveSize)
 163 {
 164     if (preserveSize)
 165         return WriteElementHead(TLVElementType::Int8, tag, static_cast<uint8_t>(v));
 166     return Put(tag, v);
 167 }
 168
 169 CHIP_ERROR TLVWriter::Put(uint64_t tag, int16_t v)
 170 {
 171     return Put(tag, static_cast<int64_t>(v));
 172 }
 173
 174 CHIP_ERROR TLVWriter::Put(uint64_t tag, int16_t v, bool preserveSize)
 175 {
 176     if (preserveSize)
 177         return WriteElementHead(TLVElementType::Int16, tag, static_cast<uint16_t>(v));
 178     return Put(tag, v);
 179 }
 180
 181 CHIP_ERROR TLVWriter::Put(uint64_t tag, int32_t v)
 182 {
 183     return Put(tag, static_cast<int64_t>(v));
 184 }
 185
 186 CHIP_ERROR TLVWriter::Put(uint64_t tag, int32_t v, bool preserveSize)
 187 {
 188     if (preserveSize)
 189         return WriteElementHead(TLVElementType::Int32, tag, static_cast<uint32_t>(v));
 190     return Put(tag, v);
 191 }
 192
 193 CHIP_ERROR TLVWriter::Put(uint64_t tag, int64_t v)
 194 {
 195     TLVElementType elemType;
 196     if (v >= INT8_MIN && v <= INT8_MAX)
 197         elemType = TLVElementType::Int8;
 198     else if (v >= INT16_MIN && v <= INT16_MAX)
 199         elemType = TLVElementType::Int16;
 200     else if (v >= INT32_MIN && v <= INT32_MAX)
 201         elemType = TLVElementType::Int32;
 202     else
 203         elemType = TLVElementType::Int64;
 204     return WriteElementHead(elemType, tag, static_cast<uint64_t>(v));
 205 }
 206
 207 CHIP_ERROR TLVWriter::Put(uint64_t tag, int64_t v, bool preserveSize)
 208 {
 209     if (preserveSize)
 210         return WriteElementHead(TLVElementType::Int64, tag, static_cast<uint64_t>(v));
 211     return Put(tag, v);
 212 }
 213
 214 CHIP_ERROR TLVWriter::Put(uint64_t tag, float v)
 215 {
 216     union
 217     {
 218         float f;
 219         uint32_t u32;
 220     } cvt;
 221     cvt.f = v;
 222     return WriteElementHead(TLVElementType::FloatingPointNumber32, tag, cvt.u32);
 223 }
 224
 225 CHIP_ERROR TLVWriter::Put(uint64_t tag, double v)
 226 {
 227     union
 228     {
 229         double d;
 230         uint64_t u64;
 231     } cvt;
 232     cvt.d = v;
 233     return WriteElementHead(TLVElementType::FloatingPointNumber64, tag, cvt.u64);
 234 }
 235
 236 CHIP_ERROR TLVWriter::Put(uint64_t tag, ByteSpan data)
 237 {
 238     VerifyOrReturnError(CanCastTo<uint32_t>(data.size()), CHIP_ERROR_MESSAGE_TOO_LONG);
 239     return PutBytes(tag, data.data(), static_cast<uint32_t>(data.size()));
 240 }
 241
 242 CHIP_ERROR TLVWriter::PutBytes(uint64_t tag, const uint8_t * buf, uint32_t len)
 243 {
 244     return WriteElementWithData(kTLVType_ByteString, tag, buf, len);
 245 }
 246
 247 CHIP_ERROR TLVWriter::PutString(uint64_t tag, const char * buf)
 248 {
 249     size_t len = strlen(buf);
 250     if (!CanCastTo<uint32_t>(len))
 251     {
 252         return CHIP_ERROR_INVALID_ARGUMENT;
 253     }
 254     return PutString(tag, buf, static_cast<uint32_t>(len));
 255 }
 256
 257 CHIP_ERROR TLVWriter::PutString(uint64_t tag, const char * buf, uint32_t len)
 258 {
 259     return WriteElementWithData(kTLVType_UTF8String, tag, reinterpret_cast<const uint8_t *>(buf), len);
 260 }
 261
 262 CHIP_ERROR TLVWriter::PutStringF(uint64_t tag, const char * fmt, ...)
 263 {
 264     CHIP_ERROR err;
 265     va_list ap;
 266
 267     va_start(ap, fmt);
 268
 269     err = VPutStringF(tag, fmt, ap);
 270
 271     va_end(ap);
 272
 273     return err;
 274 }
 275
 276 #if CONFIG_HAVE_VCBPRINTF
 277 // We have a variant of the printf function that takes a callback that
 278 // emits a single character.  The callback performs a function
 279 // identical to putchar.
 280
 281 void TLVWriter::CHIPTLVWriterPutcharCB(uint8_t c, void * appState)
 282 {
 283     TLVWriter * w = static_cast<TLVWriter *>(appState);
 284     w->WriteData(&c, sizeof(c));
 285 }
 286 #endif
 287
 288 CHIP_ERROR TLVWriter::VPutStringF(uint64_t tag, const char * fmt, va_list ap)
 289 {
 290     va_list aq;
 291     size_t dataLen;
 292     CHIP_ERROR err = CHIP_NO_ERROR;
 293     TLVFieldSize lenFieldSize;
 294 #if CONFIG_HAVE_VSNPRINTF_EX
 295     size_t skipLen;
 296     size_t writtenBytes;
 297 #elif CONFIG_HAVE_VCBPRINTF
 298 #elif CONFIG_TLV_TRUNCATE
 299     size_t maxLen;
 300 #else
 301     char * tmpBuf;
 302 #endif
 303     va_copy(aq, ap);
 304
 305     dataLen = static_cast<size_t>(vsnprintf(nullptr, 0, fmt, aq));
 306     if (!CanCastTo<uint32_t>(dataLen))
 307     {
 308         return CHIP_ERROR_INVALID_ARGUMENT;
 309     }
 310
 311     va_end(aq);
 312
 313     if (dataLen <= UINT8_MAX)
 314         lenFieldSize = kTLVFieldSize_1Byte;
 315     else if (dataLen <= UINT16_MAX)
 316         lenFieldSize = kTLVFieldSize_2Byte;
 317     else
 318         lenFieldSize = kTLVFieldSize_4Byte;
 319
 320 #if !(CONFIG_HAVE_VCBPRINTF) && !(CONFIG_HAVE_VSNPRINTF_EX) && CONFIG_TLV_TRUNCATE
 321     // no facilities for splitting the stream across multiple buffers,
 322     // just write however much fits in the current buffer.
 323     // assume conservative tag length at this time (8 bytes)
 324     maxLen = mRemainingLen -
 325         (1 + 8 + (1 << static_cast<uint8_t>(lenFieldSize)) +
 326          1); // 1 : control byte, 8 : tag length, stringLen + 1 for null termination
 327     if (maxLen < dataLen)
 328         dataLen = maxLen;
 329 #endif
 330
 331     // write length.
 332     err = WriteElementHead(
 333         static_cast<TLVElementType>(static_cast<uint8_t>(kTLVType_UTF8String) | static_cast<uint8_t>(lenFieldSize)), tag, dataLen);
 334     SuccessOrExit(err);
 335
 336     VerifyOrExit((mLenWritten + dataLen) <= mMaxLen, err = CHIP_ERROR_BUFFER_TOO_SMALL);
 337
 338     // write data
 339 #if CONFIG_HAVE_VSNPRINTF_EX
 340
 341     skipLen = 0;
 342
 343     do
 344     {
 345         va_copy(aq, ap);
 346
 347         vsnprintf_ex(reinterpret_cast<char *>(mWritePoint), mRemainingLen, skipLen, fmt, aq);
 348
 349         va_end(aq);
 350
 351         writtenBytes = (mRemainingLen >= (dataLen - skipLen)) ? dataLen - skipLen : mRemainingLen;
 352         skipLen += writtenBytes;
 353         mWritePoint += writtenBytes;
 354         mRemainingLen -= writtenBytes;
 355         mLenWritten += writtenBytes;
 356         if (skipLen < dataLen)
 357         {
 358             VerifyOrExit(mBackingStore != NULL, err = CHIP_ERROR_NO_MEMORY);
 359
 360             err = mBackingStore->FinalizeBuffer(*this, mBufHandle, mBufStart, mWritePoint - mBufStart);
 361             SuccessOrExit(err);
 362
 363             err = mBackingStore->GetNewBuffer(*this, mBufHandle, mBufStart, mRemainingLen);
 364             SuccessOrExit(err);
 365
 366             mWritePoint = mBufStart;
 367         }
 368
 369     } while (skipLen < dataLen);
 370
 371 #elif CONFIG_HAVE_VCBPRINTF
 372
 373     va_copy(aq, ap);
 374
 375     vcbprintf(CHIPTLVWriterPutcharCB, this, dataLen, fmt, aq);
 376
 377     va_end(aq);
 378
 379 #else // CONFIG_HAVE_VSNPRINTF_EX
 380
 381     tmpBuf = static_cast<char *>(chip::Platform::MemoryAlloc(dataLen + 1));
 382     VerifyOrExit(tmpBuf != nullptr, err = CHIP_ERROR_NO_MEMORY);
 383
 384     va_copy(aq, ap);
 385
 386     vsnprintf(tmpBuf, dataLen + 1, fmt, aq);
 387
 388     va_end(aq);
 389
 390     err = WriteData(reinterpret_cast<uint8_t *>(tmpBuf), static_cast<uint32_t>(dataLen));
 391     chip::Platform::MemoryFree(tmpBuf);
 392
 393 #endif // CONFIG_HAVE_VSNPRINTF_EX
 394
 395 exit:
 396
 397     return err;
 398 }
 399
 400 CHIP_ERROR TLVWriter::PutNull(uint64_t tag)
 401 {
 402     return WriteElementHead(TLVElementType::Null, tag, 0);
 403 }
 404
 405 CHIP_ERROR TLVWriter::CopyElement(TLVReader & reader)
 406 {
 407     return CopyElement(reader.GetTag(), reader);
 408 }
 409
 410 const size_t kCHIPTLVCopyChunkSize = 16;
 411
 412 CHIP_ERROR TLVWriter::CopyElement(uint64_t tag, TLVReader & reader)
 413 {
 414     TLVElementType elemType = reader.ElementType();
 415     uint64_t elemLenOrVal   = reader.mElemLenOrVal;
 416     TLVReader readerHelper; // used to figure out the length of the element and read data of the element
 417     uint32_t copyDataLen;
 418     uint8_t chunk[kCHIPTLVCopyChunkSize];
 419
 420     VerifyOrReturnError(elemType != TLVElementType::NotSpecified && elemType != TLVElementType::EndOfContainer,
 421                         CHIP_ERROR_INCORRECT_STATE);
 422
 423     // Initialize the helper
 424     readerHelper.Init(reader);
 425
 426     // Skip to the end of the element.
 427     ReturnErrorOnFailure(reader.Skip());
 428
 429     // Compute the amount of value data to copy from the reader.
 430     copyDataLen = reader.GetLengthRead() - readerHelper.GetLengthRead();
 431
 432     // Write the head of the new element with the same type and length/value, but using the
 433     // specified tag.
 434     ReturnErrorOnFailure(WriteElementHead(elemType, tag, elemLenOrVal));
 435
 436     while (copyDataLen > 0)
 437     {
 438         uint32_t chunkSize = copyDataLen > kCHIPTLVCopyChunkSize ? kCHIPTLVCopyChunkSize : copyDataLen;
 439         ReturnErrorOnFailure(readerHelper.ReadData(chunk, chunkSize));
 440
 441         ReturnErrorOnFailure(WriteData(chunk, chunkSize));
 442
 443         copyDataLen -= chunkSize;
 444     }
 445
 446     return CHIP_NO_ERROR;
 447 }
 448
 449 CHIP_ERROR TLVWriter::OpenContainer(uint64_t tag, TLVType containerType, TLVWriter & containerWriter)
 450 {
 451     CHIP_ERROR err = CHIP_NO_ERROR;
 452
 453     VerifyOrReturnError(TLVTypeIsContainer(containerType), CHIP_ERROR_WRONG_TLV_TYPE);
 454
 455     if (IsCloseContainerReserved())
 456     {
 457         VerifyOrReturnError(mMaxLen >= kEndOfContainerMarkerSize, CHIP_ERROR_BUFFER_TOO_SMALL);
 458         mMaxLen -= kEndOfContainerMarkerSize;
 459     }
 460     err = WriteElementHead(static_cast<TLVElementType>(containerType), tag, 0);
 461
 462     if (err != CHIP_NO_ERROR)
 463     {
 464         // undo the space reservation, as the container is not actually open
 465         if (IsCloseContainerReserved())
 466             mMaxLen += kEndOfContainerMarkerSize;
 467
 468         return err;
 469     }
 470
 471     containerWriter.mBackingStore  = mBackingStore;
 472     containerWriter.mBufStart      = mBufStart;
 473     containerWriter.mWritePoint    = mWritePoint;
 474     containerWriter.mRemainingLen  = mRemainingLen;
 475     containerWriter.mLenWritten    = 0;
 476     containerWriter.mMaxLen        = mMaxLen - mLenWritten;
 477     containerWriter.mContainerType = containerType;
 478     containerWriter.SetContainerOpen(false);
 479     containerWriter.SetCloseContainerReserved(IsCloseContainerReserved());
 480     containerWriter.ImplicitProfileId = ImplicitProfileId;
 481
 482     SetContainerOpen(true);
 483
 484     return CHIP_NO_ERROR;
 485 }
 486
 487 CHIP_ERROR TLVWriter::CloseContainer(TLVWriter & containerWriter)
 488 {
 489     if (!TLVTypeIsContainer(containerWriter.mContainerType))
 490         return CHIP_ERROR_INCORRECT_STATE;
 491
 492     if (containerWriter.IsContainerOpen())
 493         return CHIP_ERROR_TLV_CONTAINER_OPEN;
 494
 495     mBackingStore = containerWriter.mBackingStore;
 496     mBufStart     = containerWriter.mBufStart;
 497     mWritePoint   = containerWriter.mWritePoint;
 498     mRemainingLen = containerWriter.mRemainingLen;
 499     mLenWritten += containerWriter.mLenWritten;
 500
 501     if (IsCloseContainerReserved())
 502         mMaxLen += kEndOfContainerMarkerSize;
 503
 504     SetContainerOpen(false);
 505
 506     // Reset the container writer so that it can't accidentally be used again.
 507     containerWriter.Init(static_cast<uint8_t *>(nullptr), 0);
 508
 509     return WriteElementHead(TLVElementType::EndOfContainer, AnonymousTag, 0);
 510 }
 511
 512 CHIP_ERROR TLVWriter::StartContainer(uint64_t tag, TLVType containerType, TLVType & outerContainerType)
 513 {
 514     CHIP_ERROR err = CHIP_NO_ERROR;
 515
 516     VerifyOrReturnError(TLVTypeIsContainer(containerType), CHIP_ERROR_WRONG_TLV_TYPE);
 517
 518     if (IsCloseContainerReserved())
 519     {
 520         VerifyOrReturnError(mMaxLen >= kEndOfContainerMarkerSize, CHIP_ERROR_BUFFER_TOO_SMALL);
 521         mMaxLen -= kEndOfContainerMarkerSize;
 522     }
 523
 524     err = WriteElementHead(static_cast<TLVElementType>(containerType), tag, 0);
 525     if (err != CHIP_NO_ERROR)
 526     {
 527         // undo the space reservation, as the container is not actually open
 528         if (IsCloseContainerReserved())
 529             mMaxLen += kEndOfContainerMarkerSize;
 530
 531         return err;
 532     }
 533     outerContainerType = mContainerType;
 534     mContainerType     = containerType;
 535
 536     SetContainerOpen(false);
 537
 538     return CHIP_NO_ERROR;
 539 }
 540
 541 CHIP_ERROR TLVWriter::EndContainer(TLVType outerContainerType)
 542 {
 543     if (!TLVTypeIsContainer(mContainerType))
 544         return CHIP_ERROR_INCORRECT_STATE;
 545
 546     mContainerType = outerContainerType;
 547
 548     if (IsCloseContainerReserved())
 549         mMaxLen += kEndOfContainerMarkerSize;
 550
 551     return WriteElementHead(TLVElementType::EndOfContainer, AnonymousTag, 0);
 552 }
 553
 554 CHIP_ERROR TLVWriter::PutPreEncodedContainer(uint64_t tag, TLVType containerType, const uint8_t * data, uint32_t dataLen)
 555 {
 556     if (!TLVTypeIsContainer(containerType))
 557         return CHIP_ERROR_INVALID_ARGUMENT;
 558
 559     CHIP_ERROR err = WriteElementHead(static_cast<TLVElementType>(containerType), tag, 0);
 560     if (err != CHIP_NO_ERROR)
 561         return err;
 562
 563     return WriteData(data, dataLen);
 564 }
 565
 566 CHIP_ERROR TLVWriter::CopyContainer(TLVReader & container)
 567 {
 568     return CopyContainer(container.GetTag(), container);
 569 }
 570
 571 CHIP_ERROR TLVWriter::CopyContainer(uint64_t tag, TLVReader & container)
 572 {
 573     // NOTE: This function MUST be used with a TVLReader that is reading from a contiguous buffer.
 574     if (container.mBackingStore != nullptr)
 575         return CHIP_ERROR_INVALID_ARGUMENT;
 576
 577     CHIP_ERROR err;
 578     TLVType containerType, outerContainerType;
 579     const uint8_t * containerStart;
 580
 581     containerType = container.GetType();
 582
 583     err = container.EnterContainer(outerContainerType);
 584     if (err != CHIP_NO_ERROR)
 585         return err;
 586
 587     containerStart = container.GetReadPoint();
 588
 589     err = container.ExitContainer(outerContainerType);
 590     if (err != CHIP_NO_ERROR)
 591         return err;
 592
 593     return PutPreEncodedContainer(tag, containerType, containerStart,
 594                                   static_cast<uint32_t>(container.GetReadPoint() - containerStart));
 595 }
 596
 597 CHIP_ERROR TLVWriter::CopyContainer(uint64_t tag, const uint8_t * encodedContainer, uint16_t encodedContainerLen)
 598 {
 599     TLVReader reader;
 600
 601     reader.Init(encodedContainer, encodedContainerLen);
 602
 603     ReturnErrorOnFailure(reader.Next());
 604
 605     ReturnErrorOnFailure(PutPreEncodedContainer(tag, reader.GetType(), reader.GetReadPoint(), reader.GetRemainingLength()));
 606
 607     return CHIP_NO_ERROR;
 608 }
 609
 610 CHIP_ERROR TLVWriter::WriteElementHead(TLVElementType elemType, uint64_t tag, uint64_t lenOrVal)
 611 {
 612     uint8_t * p;
 613     uint8_t stagingBuf[17]; // 17 = 1 control byte + 8 tag bytes + 8 length/value bytes
 614
 615     if (IsContainerOpen())
 616         return CHIP_ERROR_TLV_CONTAINER_OPEN;
 617
 618     uint32_t tagNum = TagNumFromTag(tag);
 619
 620     if ((mRemainingLen >= sizeof(stagingBuf)) && (mMaxLen >= sizeof(stagingBuf)))
 621         p = mWritePoint;
 622     else
 623         p = stagingBuf;
 624
 625     if (IsSpecialTag(tag))
 626     {
 627         if (tagNum < 256)
 628         {
 629             if (mContainerType != kTLVType_Structure && mContainerType != kTLVType_List)
 630                 return CHIP_ERROR_INVALID_TLV_TAG;
 631
 632             Write8(p, TLVTagControl::ContextSpecific | elemType);
 633             Write8(p, static_cast<uint8_t>(tagNum));
 634         }
 635         else
 636         {
 637             if (elemType != TLVElementType::EndOfContainer && mContainerType != kTLVType_NotSpecified &&
 638                 mContainerType != kTLVType_Array && mContainerType != kTLVType_List)
 639                 return CHIP_ERROR_INVALID_TLV_TAG;
 640
 641             Write8(p, TLVTagControl::Anonymous | elemType);
 642         }
 643     }
 644     else
 645     {
 646         uint32_t profileId = ProfileIdFromTag(tag);
 647
 648         if (mContainerType != kTLVType_NotSpecified && mContainerType != kTLVType_Structure && mContainerType != kTLVType_List)
 649             return CHIP_ERROR_INVALID_TLV_TAG;
 650
 651         if (profileId == kCommonProfileId)
 652         {
 653             if (tagNum < 65536)
 654             {
 655                 Write8(p, TLVTagControl::CommonProfile_2Bytes | elemType);
 656                 LittleEndian::Write16(p, static_cast<uint16_t>(tagNum));
 657             }
 658             else
 659             {
 660                 Write8(p, TLVTagControl::CommonProfile_4Bytes | elemType);
 661                 LittleEndian::Write32(p, tagNum);
 662             }
 663         }
 664         else if (profileId == ImplicitProfileId)
 665         {
 666             if (tagNum < 65536)
 667             {
 668                 Write8(p, TLVTagControl::ImplicitProfile_2Bytes | elemType);
 669                 LittleEndian::Write16(p, static_cast<uint16_t>(tagNum));
 670             }
 671             else
 672             {
 673                 Write8(p, TLVTagControl::ImplicitProfile_4Bytes | elemType);
 674                 LittleEndian::Write32(p, tagNum);
 675             }
 676         }
 677         else
 678         {
 679             uint16_t vendorId   = static_cast<uint16_t>(profileId >> 16);
 680             uint16_t profileNum = static_cast<uint16_t>(profileId);
 681
 682             if (tagNum < 65536)
 683             {
 684                 Write8(p, TLVTagControl::FullyQualified_6Bytes | elemType);
 685                 LittleEndian::Write16(p, vendorId);
 686                 LittleEndian::Write16(p, profileNum);
 687                 LittleEndian::Write16(p, static_cast<uint16_t>(tagNum));
 688             }
 689             else
 690             {
 691                 Write8(p, TLVTagControl::FullyQualified_8Bytes | elemType);
 692                 LittleEndian::Write16(p, vendorId);
 693                 LittleEndian::Write16(p, profileNum);
 694                 LittleEndian::Write32(p, tagNum);
 695             }
 696         }
 697     }
 698
 699     switch (GetTLVFieldSize(elemType))
 700     {
 701     case kTLVFieldSize_0Byte:
 702         break;
 703     case kTLVFieldSize_1Byte:
 704         Write8(p, static_cast<uint8_t>(lenOrVal));
 705         break;
 706     case kTLVFieldSize_2Byte:
 707         LittleEndian::Write16(p, static_cast<uint16_t>(lenOrVal));
 708         break;
 709     case kTLVFieldSize_4Byte:
 710         LittleEndian::Write32(p, static_cast<uint32_t>(lenOrVal));
 711         break;
 712     case kTLVFieldSize_8Byte:
 713         LittleEndian::Write64(p, lenOrVal);
 714         break;
 715     }
 716
 717     if ((mRemainingLen >= sizeof(stagingBuf)) && (mMaxLen >= sizeof(stagingBuf)))
 718     {
 719         uint32_t len = static_cast<uint32_t>(p - mWritePoint);
 720         mWritePoint  = p;
 721         mRemainingLen -= len;
 722         mLenWritten += len;
 723         return CHIP_NO_ERROR;
 724     }
 725     return WriteData(stagingBuf, static_cast<uint32_t>(p - stagingBuf));
 726 }
 727
 728 CHIP_ERROR TLVWriter::WriteElementWithData(TLVType type, uint64_t tag, const uint8_t * data, uint32_t dataLen)
 729 {
 730     if (static_cast<uint64_t>(type) & kTLVTypeSizeMask)
 731     {
 732         // We won't be able to recover this type properly!
 733         return CHIP_ERROR_INVALID_ARGUMENT;
 734     }
 735
 736     TLVFieldSize lenFieldSize;
 737
 738     if (dataLen <= UINT8_MAX)
 739         lenFieldSize = kTLVFieldSize_1Byte;
 740     else if (dataLen <= UINT16_MAX)
 741         lenFieldSize = kTLVFieldSize_2Byte;
 742     else
 743         lenFieldSize = kTLVFieldSize_4Byte;
 744
 745     CHIP_ERROR err = WriteElementHead(static_cast<TLVElementType>(static_cast<uint8_t>(type) | static_cast<uint8_t>(lenFieldSize)),
 746                                       tag, dataLen);
 747     if (err != CHIP_NO_ERROR)
 748         return err;
 749
 750     return WriteData(data, dataLen);
 751 }
 752
 753 CHIP_ERROR TLVWriter::WriteData(const uint8_t * p, uint32_t len)
 754 {
 755     VerifyOrReturnError((mLenWritten + len) <= mMaxLen, CHIP_ERROR_BUFFER_TOO_SMALL);
 756
 757     while (len > 0)
 758     {
 759         if (mRemainingLen == 0)
 760         {
 761             VerifyOrReturnError(mBackingStore != nullptr, CHIP_ERROR_NO_MEMORY);
 762
 763             VerifyOrReturnError(CanCastTo<uint32_t>(mWritePoint - mBufStart), CHIP_ERROR_INCORRECT_STATE);
 764             ReturnErrorOnFailure(mBackingStore->FinalizeBuffer(*this, mBufStart, static_cast<uint32_t>(mWritePoint - mBufStart)));
 765
 766             ReturnErrorOnFailure(mBackingStore->GetNewBuffer(*this, mBufStart, mRemainingLen));
 767
 768             mWritePoint = mBufStart;
 769
 770             if (mRemainingLen > (mMaxLen - mLenWritten))
 771                 mRemainingLen = (mMaxLen - mLenWritten);
 772         }
 773
 774         uint32_t writeLen = len;
 775         if (writeLen > mRemainingLen)
 776             writeLen = mRemainingLen;
 777
 778         memmove(mWritePoint, p, writeLen);
 779         mWritePoint += writeLen;
 780         mRemainingLen -= writeLen;
 781         mLenWritten += writeLen;
 782         p += writeLen;
 783         len -= writeLen;
 784     }
 785
 786     return CHIP_NO_ERROR;
 787 }
 788
 789 } // namespace TLV
 790 } // namespace chip