space_packet.c

[詳解]

#pragma section REPRO
#include "space_packet.h"
#include "../../Library/endian.h"
#include <string.h>
 
 
static const SP_ParamExtractionInfo SP_pei_ver_           = { 0, 0xe0, 5, 1};  // 11100000b
static const SP_ParamExtractionInfo SP_pei_type_          = { 0, 0x10, 4, 1};  // 00010000b
static const SP_ParamExtractionInfo SP_pei_2nd_hdr_flag_  = { 0, 0x08, 3, 1};  // 00001000b
static const SP_ParamExtractionInfo SP_pei_apid_          = { 0, 0x07, 0, 2};  // 00000111b
static const SP_ParamExtractionInfo SP_pei_seq_flag_      = { 2, 0xc0, 6, 1};  // 11000000b
static const SP_ParamExtractionInfo SP_pei_seq_count_     = { 2, 0x3f, 0, 2};  // 00111111b
static const SP_ParamExtractionInfo SP_pei_pckt_data_len_ = { 4, 0xff, 0, 2};  // 11111111b
 
 
// ******************************
//  Primary Header getter/setter
// ******************************
 
SP_VER SP_get_ver(const SpacePacket* sp)
{
  uint8_t tmp;
  SP_VER ver;
 
  SP_extract_param_from_packet(sp, &SP_pei_ver_, &tmp);
  ver = (SP_VER)tmp;
 
  switch (ver)
  {
  case SP_VER_1:
    return ver;
  default:
    return SP_VER_UNKNOWN;
  }
}
 
 
void SP_set_ver(SpacePacket* sp, SP_VER ver)
{
  uint8_t tmp = (uint8_t)ver;
  SP_insert_param_to_packet(sp, &SP_pei_ver_, &tmp);
}
 
 
SP_TYPE SP_get_type(const SpacePacket* sp)
{
  uint8_t type;
 
  SP_extract_param_from_packet(sp, &SP_pei_type_, &type);
  return (SP_TYPE)type;
}
 
 
void SP_set_type(SpacePacket* sp, SP_TYPE type)
{
  uint8_t tmp = (uint8_t)type;
  SP_insert_param_to_packet(sp, &SP_pei_type_, &tmp);
}
 
 
SP_2ND_HDR_FLAG SP_get_2nd_hdr_flag(const SpacePacket* sp)
{
  uint8_t flag;
 
  SP_extract_param_from_packet(sp, &SP_pei_2nd_hdr_flag_, &flag);
  return (SP_2ND_HDR_FLAG)flag;
}
 
 
void SP_set_2nd_hdr_flag(SpacePacket* sp, SP_2ND_HDR_FLAG flag)
{
  uint8_t tmp = (uint8_t)flag;
  SP_insert_param_to_packet(sp, &SP_pei_2nd_hdr_flag_, &tmp);
}
 
 
APID SP_get_apid(const SpacePacket* sp)
{
  uint16_t apid;
  SP_extract_param_from_packet(sp, &SP_pei_apid_, &apid);
  return APID_get_apid_from_uint16(apid);
}
 
 
void SP_set_apid(SpacePacket* sp, APID apid)
{
  uint16_t tmp = (uint16_t)apid;
  SP_insert_param_to_packet(sp, &SP_pei_apid_, &tmp);
}
 
 
SP_SEQ_FLAG SP_get_seq_flag(const SpacePacket* sp)
{
  uint8_t flag;
 
  SP_extract_param_from_packet(sp, &SP_pei_seq_flag_, &flag);
  return (SP_SEQ_FLAG)flag;
}
 
 
void SP_set_seq_flag(SpacePacket* sp, SP_SEQ_FLAG flag)
{
  uint8_t tmp = (uint8_t)flag;
  SP_insert_param_to_packet(sp, &SP_pei_seq_flag_, &tmp);
}
 
 
uint16_t SP_get_seq_count(const SpacePacket* sp)
{
  uint16_t count;
 
  SP_extract_param_from_packet(sp, &SP_pei_seq_count_, &count);
  return count;
}
 
 
void SP_set_seq_count(SpacePacket* sp, uint16_t count)
{
  SP_insert_param_to_packet(sp, &SP_pei_seq_count_, &count);
}
 
 
uint16_t SP_get_packet_data_len(const SpacePacket* sp)
{
  uint16_t len;
 
  SP_extract_param_from_packet(sp, &SP_pei_pckt_data_len_, &len);
  // Data Length は 0 起算表記なので 1 起算に変換して返す
  return (uint16_t)(len + 1);
}
 
 
void SP_set_packet_data_len(SpacePacket* sp, uint16_t len)
{
  if (len == 0) return;
 
  // Data Length は 0 起算表記なので 1 起算を変換
  len--;
  SP_insert_param_to_packet(sp, &SP_pei_pckt_data_len_, &len);
}
 
 
// ******************
//  Utility Function
// ******************
 
uint16_t SP_get_packet_len(const SpacePacket* sp)
{
  return (uint16_t)(SP_get_packet_data_len(sp) + SP_PRM_HDR_LEN);
}
 
 
void SP_set_packet_len(SpacePacket* sp, uint16_t len)
{
  if (len <= SP_PRM_HDR_LEN) return;
  SP_set_packet_data_len(sp, (uint16_t)(len - SP_PRM_HDR_LEN));
}
 
 
void SP_copy_packet(SpacePacket* dest, const SpacePacket* src)
{
  memcpy(dest->packet, src->packet, (size_t)SP_get_packet_len(src));
}
 
 
void SP_extract_param_from_packet(const SpacePacket* sp,
                                  const SP_ParamExtractionInfo* pei,
                                  void* dst)
{
  uint8_t buffer[8];
  uint8_t i;
 
  if (pei->byte_len == 0) return;
  if (pei->byte_len > 8) return;
  if (pei->offset > 7) return;
 
  // offset あり
  if (pei->offset)
  {
    // offset されるのは， 1byte の中で格納される時
    if (pei->byte_len != 1) return;
 
    buffer[0] = (sp->packet[pei->pos] & pei->mask) >> pei->offset;
  }
  else
  {
    // offset なし
    // 最初の 1byte
    buffer[0] = sp->packet[pei->pos] & pei->mask;
 
    // 2byte 目以降
    for (i = 1; i < pei->byte_len; ++i)
    {
      buffer[i] = sp->packet[pei->pos + i];
    }
  }
 
  ENDIAN_memcpy(dst, buffer, pei->byte_len);
}
 
 
void SP_insert_param_to_packet(SpacePacket* sp,
                               const SP_ParamExtractionInfo* pei,
                               const void* src)
{
  uint8_t buffer[8];
  uint8_t i;
 
  if (pei->byte_len == 0) return;
  if (pei->byte_len > 8) return;
  if (pei->offset > 7) return;
 
  ENDIAN_memcpy(buffer, src, pei->byte_len);
 
  // offset あり
  if (pei->offset)
  {
    // offset されるのは， 1byte の中で格納される時
    if (pei->byte_len != 1) return;
 
    buffer[0] <<= pei->offset;
 
    sp->packet[pei->pos] &= (uint8_t)(~pei->mask);
    sp->packet[pei->pos] |= (uint8_t)(buffer[0] & pei->mask);
  }
  else
  {
    // offset なし
    // 最初の 1byte
    sp->packet[pei->pos] &= (uint8_t)(~pei->mask);
    sp->packet[pei->pos] |= (uint8_t)(buffer[0] & pei->mask);
 
    // 2byte 目以降
    for (i = 1; i < pei->byte_len; ++i)
    {
      sp->packet[pei->pos + i] = buffer[i];
    }
  }
}
 
#pragma section