nlink_linktrack_nodeframe6.c

#include "nlink_linktrack_nodeframe6.h"

#include <stdio.h>

#include "nlink_utils.h"

#pragma pack(1)
typedef struct {
  uint8_t header[2];
  uint16_t frame_length;
  uint8_t role;
  id_t id;
  uint8_t reserved[4];
  uint8_t valid_node_count;
  // nodes...
  // uint8_t checkSum;
} nlt_nodeframe6_raw_t;
#pragma pack()
static nlt_nodeframe6_raw_t g_frame;

static uint8_t UnpackData(const uint8_t *data, size_t data_length) {
  if (data_length < g_nlt_nodeframe6.fixed_part_size ||
      data[0] != g_nlt_nodeframe6.frame_header ||
      data[1] != g_nlt_nodeframe6.function_mark)
    return 0;
  size_t frame_length = NLINK_PROTOCOL_LENGTH(data);
  if (data_length < frame_length)
    return 0;
  if (!NLINK_VerifyCheckSum(data, frame_length))
    return 0;

  static uint8_t init_needed = 1;
  if (init_needed) {
    memset(g_nlt_nodeframe6.result.nodes, 0,
           sizeof(g_nlt_nodeframe6.result.nodes));
    init_needed = 0;
  }

  memcpy(&g_frame, data, g_nlt_nodeframe6.fixed_part_size);
  g_nlt_nodeframe6.result.role = g_frame.role;
  g_nlt_nodeframe6.result.id = g_frame.id;
  g_nlt_nodeframe6.result.valid_node_count = g_frame.valid_node_count;

  for (size_t i = 0, address = g_nlt_nodeframe6.fixed_part_size,
              roleIdSize = 1 + sizeof(id_t);
       i < g_frame.valid_node_count; ++i) {
    const uint8_t *begin = data + address;
    size_t data_length =
        (size_t)(begin[roleIdSize] | begin[roleIdSize + 1] << 8);

    size_t current_node_size = roleIdSize + 2 + data_length;
    if (address + current_node_size > frame_length - 1) {
      printf("warning: address(%zu) + current_node_size(%zu) > "
             "frame_length(%zu)-1",
             address, current_node_size, frame_length);
      return 0;
    }

    TRY_MALLOC_NEW_NODE(g_nlt_nodeframe6.result.nodes[i], nlt_nodeframe6_node_t)
    nlt_nodeframe6_node_t *node = g_nlt_nodeframe6.result.nodes[i];
    node->role = begin[0];
    memcpy(&node->id, begin + 1, sizeof(node->id));
    node->data_length = data_length;
    memcpy(node->data, begin + roleIdSize + 2, data_length);

    address += current_node_size;
  }

  return 1;
}

nlt_nodeframe6_t g_nlt_nodeframe6 = {.fixed_part_size = 14,
                                     .frame_header = 0x55,
                                     .function_mark = 0x09,
                                     .UnpackData = UnpackData};