PL_CDR support (#18)

* - add pl_cdr1 header * - separate sentinel header handling - writer support for extended PID - tests * - use constants for reserved PIDs * - fix typo * - feedback
foxglove · Aug 21, 2023 · 10451c7 · 10451c7
1 parent 9ec60d9
commit 10451c7
Show file tree

Hide file tree

Showing 6 changed files with 209 additions and 4 deletions.
diff --git a/src/CdrReader.test.ts b/src/CdrReader.test.ts
@@ -199,6 +199,46 @@ describe("CdrReader", () => {
     expect(reader.offset).toEqual(writer.data.length);
   });
 
+  it.each([
+    [true, 100, 1],
+    [false, 200, 2],
+    [false, 1028, 4],
+    [false, 65, 8],
+    [true, 63, 9],
+    [false, 127, 0xffff],
+    [false, 127, 0x1ffff], // extended PID
+    [true, 700000, 0xffff], // extended PID
+    [false, 700000, 0x1ffff], // extended PID
+  ])(
+    "round trips XCDR1 parameter header values with mustUnderstand: %d, id: %d, and size: %d",
+    (mustUnderstand: boolean, id: number, objectSize: number) => {
+      const writer = new CdrWriter({ kind: EncapsulationKind.PL_CDR_BE });
+
+      writer.emHeader(mustUnderstand, id, objectSize);
+
+      const reader = new CdrReader(writer.data);
+      const header = reader.emHeader();
+
+      expect(header).toEqual({
+        objectSize,
+        id,
+        mustUnderstand,
+      });
+    },
+  );
+
+  it("converts extended PID", () => {
+    const buffer = new Uint8Array(Buffer.from("00030000017f080064000000400000000", "hex"));
+    const reader = new CdrReader(buffer);
+    expect(reader.emHeader()).toMatchInlineSnapshot(`
+Object {
+  "id": 100,
+  "mustUnderstand": true,
+  "objectSize": 64,
+}
+`);
+  });
+
   it("takes a length when reading a string and doesn't read the sequence length again", () => {
     const writer = new CdrWriter();
     const testString = "test";
@@ -208,6 +248,15 @@ describe("CdrReader", () => {
     const length = reader.sequenceLength();
     expect(reader.string(length)).toEqual("test");
   });
+
+  it("errors when expecting to read a sentinel header but receives non-sentinel_PID value", () => {
+    const writer = new CdrWriter({ kind: EncapsulationKind.PL_CDR_LE });
+    writer.emHeader(false, 100, 4);
+
+    const reader = new CdrReader(writer.data);
+    expect(() => reader.sentinelHeader()).toThrowError(/Expected sentinel_pid/i);
+  });
+
   it.each([[1], [2], [4], [8], [0x7fffffff]])(
     "round trips DHEADER values of size %d",
     (objectSize) => {

diff --git a/src/CdrReader.ts b/src/CdrReader.ts
@@ -1,6 +1,7 @@
 import { EncapsulationKind } from "./EncapsulationKind";
 import { getEncapsulationKindInfo } from "./getEncapsulationKindInfo";
 import { isBigEndian } from "./isBigEndian";
+import { EXTENDED_PID, SENTINEL_PID } from "./reservedPIDs";
 
 interface Indexable {
   [index: number]: unknown;
@@ -29,8 +30,12 @@ export class CdrReader {
   private littleEndian: boolean;
   private hostLittleEndian: boolean;
   private eightByteAlignment: number; // Alignment for 64-bit values, 4 on CDR2 8 on CDR1
+  private isCDR2: boolean;
   private textDecoder = new TextDecoder("utf8");
 
+  /** Origin offset into stream used for alignment */
+  private origin = 0;
+
   // Need to be public for higher level serializers to use
   readonly usesDelimiterHeader: boolean;
   readonly usesMemberHeader: boolean;
@@ -66,7 +71,9 @@ export class CdrReader {
 
     this.littleEndian = littleEndian;
     this.hostLittleEndian = !isBigEndian();
+    this.isCDR2 = isCDR2;
     this.eightByteAlignment = isCDR2 ? 4 : 8;
+    this.origin = 4;
     this.offset = 4;
   }
 
@@ -181,6 +188,79 @@ export class CdrReader {
    * Reads the member header (EMHEADER) and returns the member ID, mustUnderstand flag, and object size
    */
   emHeader(): { mustUnderstand: boolean; id: number; objectSize: number } {
+    if (this.isCDR2) {
+      return this.memberHeaderV2();
+    } else {
+      return this.memberHeaderV1();
+    }
+  }
+
+  /** XCDR1 PL_CDR encapsulation parameter header*/
+  private memberHeaderV1(): { id: number; objectSize: number; mustUnderstand: boolean } {
+    // 4-byte header with two 16-bit fields
+    this.align(4);
+    const idHeader = this.uint16();
+
+    const mustUnderstandFlag = (idHeader & 0x4000) >> 14 === 1;
+    // indicates that the parameter has a implementation-specific interpretation
+    const implementationSpecificFlag = (idHeader & 0x8000) >> 15 === 1;
+
+    // Allows the specification of large member ID and/or data length values
+    // requires the reading in of two uint32's for ID and size
+    const extendedPIDFlag = (idHeader & 0x3fff) === EXTENDED_PID;
+
+    // Indicates the end of the parameter list structure
+    const sentinelPIDFlag = (idHeader & 0x3fff) === SENTINEL_PID;
+    if (sentinelPIDFlag) {
+      throw Error("Expected Member Header but got SENTINEL_PID Flag");
+    }
+
+    // Indicates that the ID should be ignored
+    // const ignorePIDFlag = (idHeader & 0x3fff) === 0x3f03;
+
+    const usesReservedParameterId = (idHeader & 0x3fff) > SENTINEL_PID;
+
+    // Not trying to support right now if we don't need to
+    if (usesReservedParameterId || implementationSpecificFlag) {
+      throw new Error(`Unsupported parameter ID header ${idHeader.toString(16)}`);
+    }
+
+    if (extendedPIDFlag) {
+      // Need to consume last part of header (is just an 8 in this case)
+      // Alignment could take care of this, but I want to be explicit
+      this.uint16();
+    }
+
+    const id = extendedPIDFlag ? this.uint32() : idHeader & 0x3fff;
+    const objectSize = extendedPIDFlag ? this.uint32() : this.uint16();
+    this.resetOrigin();
+    return { id, objectSize, mustUnderstand: mustUnderstandFlag };
+  }
+
+  /** Sets the origin to the offset (DDS-XTypes Spec: `PUSH(ORIGIN = 0)`)*/
+  private resetOrigin(): void {
+    this.origin = this.offset;
+  }
+
+  /** Reads the PID_SENTINEL value if encapsulation kind supports it (PL_CDR version 1)*/
+  sentinelHeader(): void {
+    if (!this.isCDR2) {
+      this.align(4);
+      const header = this.uint16();
+      // Indicates the end of the parameter list structure
+      const sentinelPIDFlag = (header & 0x3fff) === SENTINEL_PID;
+      if (!sentinelPIDFlag) {
+        throw Error(
+          `Expected SENTINEL_PID (${SENTINEL_PID.toString(16)}) flag, but got ${header.toString(
+            16,
+          )}`,
+        );
+      }
+      this.uint16();
+    }
+  }
+
+  private memberHeaderV2(): { id: number; objectSize: number; mustUnderstand: boolean } {
     const header = this.uint32();
     // EMHEADER = (M_FLAG<<31) + (LC<<28) + M.id
     // M is the member of a structure
@@ -307,7 +387,7 @@ export class CdrReader {
   }
 
   private align(size: number): void {
-    const alignment = (this.offset - 4) % size;
+    const alignment = (this.offset - this.origin) % size;
     if (alignment > 0) {
       this.offset += size - alignment;
     }

diff --git a/src/CdrWriter.test.ts b/src/CdrWriter.test.ts
@@ -188,6 +188,29 @@ describe("CdrWriter", () => {
     writer.align(4); // no-op, already aligned
     expect(toHex(writer.data)).toEqual("000b00000100000002000000");
   });
+  it("aligns 8 byte values in xcdr PL_CDR without emheader using padding", () => {
+    const writer = new CdrWriter({ kind: EncapsulationKind.PL_CDR_LE }); // origin=  4
+    writer.uint32(1);
+    writer.uint64(0x0fn);
+    //prettier-ignore
+    expect(toHex(writer.data)).toEqual(
+      "00030000" +  // header 
+      "01000000" +  // uint32
+      "00000000" + // padding
+      "0f00000000000000" //uint64
+    );
+  });
+  it("emHeaders resets origin for 8 byte value alignment", () => {
+    const writer = new CdrWriter({ kind: EncapsulationKind.PL_CDR_LE }); // origin=  4
+    writer.emHeader(true, 5, 8);
+    writer.uint64(0x0fn);
+    //prettier-ignore
+    expect(toHex(writer.data)).toEqual(
+      "00030000" +  // header 
+      "05400800" +  // uint32
+      "0f00000000000000" //uint64
+    );
+  });
 });
 
 function enumKeys<O extends Record<string, unknown>, K extends keyof O = keyof O>(obj: O): K[] {

diff --git a/src/CdrWriter.ts b/src/CdrWriter.ts
@@ -1,6 +1,7 @@
 import { EncapsulationKind } from "./EncapsulationKind";
 import { getEncapsulationKindInfo } from "./getEncapsulationKindInfo";
 import { isBigEndian } from "./isBigEndian";
+import { EXTENDED_PID, SENTINEL_PID } from "./reservedPIDs";
 
 export type CdrWriterOpts = {
   buffer?: ArrayBuffer;
@@ -14,12 +15,15 @@ export class CdrWriter {
 
   private littleEndian: boolean;
   private hostLittleEndian: boolean;
+  private isCDR2: boolean;
   private eightByteAlignment: number; // Alignment for 64-bit values, 4 on CDR2 8 on CDR1
   private buffer: ArrayBuffer;
   private array: Uint8Array;
   private view: DataView;
   private textEncoder = new TextEncoder();
   private offset: number;
+  /** Origin offset into stream used for alignment */
+  private origin: number;
 
   get data(): Uint8Array {
     return new Uint8Array(this.buffer, 0, this.offset);
@@ -45,6 +49,7 @@ export class CdrWriter {
     const kind = options.kind ?? EncapsulationKind.CDR_LE;
 
     const { isCDR2, littleEndian } = getEncapsulationKindInfo(kind);
+    this.isCDR2 = isCDR2;
     this.littleEndian = littleEndian;
     this.hostLittleEndian = !isBigEndian();
     this.eightByteAlignment = isCDR2 ? 4 : 8;
@@ -60,6 +65,7 @@ export class CdrWriter {
     // when it reads the options field
     this.view.setUint16(2, 0, false);
     this.offset = 4;
+    this.origin = 4;
   }
 
   int8(value: number): CdrWriter {
@@ -179,8 +185,52 @@ export class CdrWriter {
 
   /**
    * Writes the member header (EMHEADER): mustUnderstand flag, the member ID, and object size
+   * Accomodates for PL_CDR and PL_CDR2 based on the CdrWriter constructor options
    */
   emHeader(mustUnderstand: boolean, id: number, objectSize: number): CdrWriter {
+    return this.isCDR2
+      ? this.memberHeaderV2(mustUnderstand, id, objectSize)
+      : this.memberHeaderV1(mustUnderstand, id, objectSize);
+  }
+
+  private memberHeaderV1(mustUnderstand: boolean, id: number, objectSize: number): CdrWriter {
+    this.align(4);
+    const mustUnderstandFlag = mustUnderstand ? 1 << 14 : 0;
+    const shouldUseExtendedPID = id > 0x3f00 || objectSize > 0xffff;
+
+    if (!shouldUseExtendedPID) {
+      const idHeader = mustUnderstandFlag | id;
+      this.uint16(idHeader);
+      const objectSizeHeader = objectSize & 0xffff;
+      this.uint16(objectSizeHeader);
+    } else {
+      const extendedHeader = mustUnderstandFlag | EXTENDED_PID;
+      this.uint16(extendedHeader);
+      this.uint16(8); // size of next two parameters
+      this.uint32(id);
+      this.uint32(objectSize);
+    }
+
+    this.resetOrigin();
+
+    return this;
+  }
+
+  /** Sets the origin to the offset (DDS-XTypes Spec: `PUSH(ORIGIN = 0)`)*/
+  private resetOrigin() {
+    this.origin = this.offset;
+  }
+
+  /** Writes the PID_SENTINEL value if encapsulation supports it*/
+  sentinelHeader(): CdrWriter {
+    if (!this.isCDR2) {
+      this.uint16(SENTINEL_PID);
+      this.uint16(0);
+    }
+    return this;
+  }
+
+  private memberHeaderV2(mustUnderstand: boolean, id: number, objectSize: number): CdrWriter {
     if (id > 0x0fffffff) {
       // first byte is used for M_FLAG and LC
       throw Error(`Member ID ${id} is too large. Max value is ${0x0fffffff}`);
@@ -421,8 +471,7 @@ export class CdrWriter {
    *   data such as arrays
    */
   align(size: number, bytesToWrite: number = size): void {
-    // The four byte header is not considered for alignment
-    const alignment = (this.offset - 4) % size;
+    const alignment = (this.offset - this.origin) % size;
     const padding = alignment > 0 ? size - alignment : 0;
     this.resizeIfNeeded(padding + bytesToWrite);
     // Write padding bytes

diff --git a/src/getEncapsulationKindInfo.ts b/src/getEncapsulationKindInfo.ts
@@ -59,8 +59,10 @@ export const getEncapsulationKindInfo = (
     kind === EncapsulationKind.RTPS_PL_CDR2_BE ||
     kind === EncapsulationKind.RTPS_PL_CDR2_LE;
 
+  const isPLCDR1 = kind === EncapsulationKind.PL_CDR_BE || kind === EncapsulationKind.PL_CDR_LE;
+
   const usesDelimiterHeader = isDelimitedCDR2 || isPLCDR2;
-  const usesMemberHeader = isPLCDR2;
+  const usesMemberHeader = isPLCDR2 || isPLCDR1;
 
   return {
     isCDR2,

diff --git a/src/reservedPIDs.ts b/src/reservedPIDs.ts
@@ -0,0 +1,2 @@
+export const EXTENDED_PID = 0x3f01;
+export const SENTINEL_PID = 0x3f02;