stmbl

// 11/2015  Rene Hopf<renehopf@mac.com> and Ian McMahon<facetious.ian@gmail.com>

//using CMD_TX
//using CMD_RX

COMP(sserial);

//dump:
//sigrok-cli --driver fx2lafw --config samplerate=24m --time 1s --channels 3=rx,2=tx -t rx=f -o sserial.sr
//decode:
//sigrok-cli -i sserial.sr -P uart:baudrate=2500000:tx=tx:rx=rx:format=hex -A uart=tx-data:rx-data
//decode more:
//sigrok-cli -i sserial.sr -P uart:baudrate=2500000:tx=tx:rx=rx,sserial

#define UnitNumberRPC 0xbc
#define DiscoveryRPC 0xbb
#define ProcessDataRPC 0xbd

//local commands
#define LBPCookieCMD 0xdf
#define LBPCookie 0x5a

#define LBPStatusCMD 0xc1

#define LBPCardName "stbl"
#define LBPCardName0Cmd 0xd0
#define LBPCardName1Cmd 0xd1
#define LBPCardName2Cmd 0xd2
#define LBPCardName3Cmd 0xd3

#define CT_RW     1 // 01b
#define CT_RPC    2 // 10b
#define CT_LOCAL  3 // 11b

typedef union{
    struct {
        uint8_t ds:2;//DataSize: Specifies data size, 00b = 1 bytes, 01b = 2 bytes, 10 b= 4 bytes, 11b = 8 bytes.
        uint8_t as:1;//AddressSize: 0 to specify current address, 1 to specify 2 byte address.
        uint8_t ai:1;//AutoInc: 0 leaves address unchanged, 1 specifies that address is post incremented by data size in bytes.
        uint8_t rid:1;//RPCIncludesData: 0 specifies that data is from stream, 1, that data is from RPC (RPC only, ignored for non RPC commands)
        uint8_t wr:1;//Write: 1 to specify write, 0 to specify read
        uint8_t ct:2;//CommandType: Must be 01b to specify data read/write command
        // 01:rw   11:local command   10:rpc
    };
    struct {
        uint8_t rpc:6;
        uint8_t dummy:2;
        //01:rw 11: local command 10:rpc
    };
    uint8_t byte;
} lbp_t;

#define MEM_SIZE 2048

#define RECORD_TYPE_PROCESS_DATA_RECORD 0xA0
#define RECORD_TYPE_MODE_DATA_RECORD 0xB0

//process data
#define DATA_TYPE_PAD 0x00
#define DATA_TYPE_BITS 0x01
#define DATA_TYPE_UNSIGNED 0x02
#define DATA_TYPE_SIGNED 0x03
#define DATA_TYPE_NONVOL_UNSIGNED 0x04
#define DATA_TYPE_NONVOL_SIGNED 0x05
#define DATA_TYPE_NONVOL_STREAM 0x06
#define DATA_TYPE_NONVOL_BOOLEAN 0x07

#define DATA_DIRECTION_INPUT 0x00
#define DATA_DIRECTION_BI_DIRECTIONAL 0x40
#define DATA_DIRECTION_OUTPUT 0x80


#define MEMPTR(p) ((uint32_t)&p-(uint32_t)&memory)

#define MEMU8(ptr) (memory.bytes[ptr])
#define MEMU16(ptr) (memory.bytes[ptr] | memory.bytes[ptr+1]<<8)
#define MEMU32(ptr) (memory.bytes[ptr] | memory.bytes[ptr+1]<<8 | memory.bytes[ptr+2]<<16 | memory.bytes[ptr+3]<<24)
#define MEMFLOAT(ptr) ((float)({uint32_t tmp = MEMU32(ptr);*((float*)&tmp);}))

#define NUM_BYTES(bits) (bits / 8 + (bits % 8 > 0 ? 1 : 0))

#define IS_INPUT(pdr) (pdr->data_direction != 0x80)
#define IS_OUTPUT(pdr) (pdr->data_direction != 0x00)

#define SIGNED(pdr) (pdr->data_type == DATA_TYPE_SIGNED || pdr->data_type == DATA_TYPE_NONVOL_SIGNED)
#define UNSIGNED(pdr) (pdr->data_type == DATA_TYPE_UNSIGNED || pdr->data_type == DATA_TYPE_NONVOL_UNSIGNED)

// pins
HAL_PIN(dump_pd_vals) = 0.0;
HAL_PIN(error) = 0.0;//counts unknown commands
HAL_PIN(crc_error) = 0.0;//counts crc errors
HAL_PIN(connected) = 0.0;//connection status TODO: not stable during startup, needs link to pd
HAL_PIN(timeout) = 100.0;// 20khz / 1khz * 2 reads = 40

HAL_PIN(pos_cmd) = 0.0;
HAL_PIN(pos_fb) = 0.0;
HAL_PIN(vel_fb) = 0.0;

HAL_PIN(in0) = 0.0;
HAL_PIN(in1) = 0.0;
HAL_PIN(in2) = 0.0;
HAL_PIN(in3) = 0.0;

HAL_PIN(out0) = 0.0;
HAL_PIN(out1) = 0.0;
HAL_PIN(out2) = 0.0;
HAL_PIN(out3) = 0.0;


typedef struct{
   uint8_t record_type;//0xa0
   uint8_t data_size;
   uint8_t data_type;
   uint8_t data_direction;
   float param_min;
   float param_max;
   uint16_t data_addr;
   char names;
} process_data_descriptor_t;

typedef struct{
   uint8_t record_type;//0xb0
   uint8_t index;
   uint8_t type;
   uint8_t unused;
   char names;
} mode_descriptor_t;

typedef struct{
   uint8_t input;  //process data input bytes 
   uint8_t output; //process data output bytes 
   uint16_t ptocp; //pointer to process data table start
   uint16_t gtocp; //pointer to mode data table start
} discovery_rpc_t;

typedef union {
  struct {
    discovery_rpc_t discovery;
    uint16_t foo;
    uint8_t heap[MEM_SIZE - sizeof(discovery_rpc_t)];
  };

  uint8_t bytes[MEM_SIZE];
} memory_t;

typedef struct {
  process_data_descriptor_t *ptr;
  float range;
  uint32_t bitmax;
} pd_metadata_t;

typedef struct {
  pd_metadata_t pos_cmd;
  pd_metadata_t scale;
  pd_metadata_t pos_fb;
  pd_metadata_t vel_fb;
  pd_metadata_t input_pins;
  pd_metadata_t output_pins;
} pd_table_t;


MEM(volatile uint8_t rxbuf[128]);
MEM(volatile uint8_t txbuf[20]);
MEM(uint16_t address);//current address pointer
MEM(int rxpos);
MEM(memory_t memory);
MEM(uint8_t *heap_ptr);
MEM(uint32_t timeout);
MEM(pd_table_t pd_table);


//pb13 txen
//pc12 usart5 tx
//pa9 usart1 tx as rx
void init_hardware() {
  GPIO_InitTypeDef GPIO_InitStruct;
  USART_InitTypeDef USART_InitStruct;
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
  //USART TX
  GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12;
  GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP ;
  GPIO_Init(GPIOC, &GPIO_InitStruct);

  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13;
  GPIO_Init(GPIOB, &GPIO_InitStructure);

  //USART RX
  GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_USART1);
  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_9;
  GPIO_Init(GPIOA, &GPIO_InitStruct);

  USART_InitStruct.USART_BaudRate = 2500000;
  USART_InitStruct.USART_WordLength = USART_WordLength_8b;
  USART_InitStruct.USART_StopBits = USART_StopBits_1;
  USART_InitStruct.USART_Parity = USART_Parity_No;
  USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStruct.USART_Mode = USART_Mode_Rx;
  USART_Init(USART1, &USART_InitStruct);
  USART_HalfDuplexCmd(USART1,ENABLE);

  USART_InitStruct.USART_Mode = USART_Mode_Tx;
  USART_Init(UART5, &USART_InitStruct);

  USART_Cmd(USART1, ENABLE);
  USART_Cmd(UART5, ENABLE);

  //RX DMA

  DMA_Cmd(DMA2_Stream5, DISABLE);
  DMA_DeInit(DMA2_Stream5);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(USART1->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&rxbuf;
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
  DMA_InitStructure.DMA_BufferSize = sizeof(rxbuf);
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
  DMA_Init(DMA2_Stream5, &DMA_InitStructure);

  DMA_Cmd(DMA2_Stream5, ENABLE);

  USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);

  //TX DMA

  DMA_Cmd(DMA1_Stream7, DISABLE);
  DMA_DeInit(DMA1_Stream7);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&txbuf;
  DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
  DMA_InitStructure.DMA_BufferSize = sizeof(txbuf);
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_Mode =  DMA_Priority_Low;
  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
  DMA_Init(DMA1_Stream7, &DMA_InitStructure);

  //DMA_Cmd(DMA1_Stream7, ENABLE);

  USART_DMACmd(UART5, USART_DMAReq_Tx, ENABLE);
   
  GPIO_SetBits(GPIOB, GPIO_Pin_13);//tx enable
}

process_data_descriptor_t create_pdr(uint8_t data_size_in_bits, uint8_t data_type, uint8_t data_dir, float param_min, float param_max) {
  process_data_descriptor_t pd;

  pd.record_type    = RECORD_TYPE_PROCESS_DATA_RECORD;
  pd.data_size    = data_size_in_bits;
  pd.data_type    = data_type;
  pd.data_direction   = data_dir;
  pd.param_min    = param_min;
  pd.param_max    = param_max;

  return pd;
}

mode_descriptor_t create_mdr(uint8_t index, uint8_t type) {
  mode_descriptor_t md;

  md.record_type = RECORD_TYPE_MODE_DATA_RECORD;
  md.index = index;
  md.type = type;
  md.unused = 0x00;

  return md;
}

uint16_t add_pd(char *name_string, char *unit_string, uint8_t data_size_in_bits, uint8_t data_type, uint8_t data_dir, float param_min, float param_max) {
  process_data_descriptor_t pdr = create_pdr(data_size_in_bits, data_type, data_dir, param_min, param_max);

  pdr.data_addr = MEMPTR(*heap_ptr);
  heap_ptr += NUM_BYTES(data_size_in_bits);
  // this aligns the heap pointer to 32bit.  Not doing this causes the floats in the pd to be misaligned, which crashes the arm.
  if((uint32_t)heap_ptr % 4){
     heap_ptr += 4 - (uint32_t)heap_ptr % 4;
  }

  memcpy(heap_ptr, &pdr, sizeof(process_data_descriptor_t));
  // note that we don't store the names in the struct anymore.  The fixed-length struct is copied into memory, and then the nmaes go in directly behind it, so they'll read out properly

  uint16_t pd_ptr = MEMPTR(*heap_ptr); // save off the ptr to return, before we modify the heap ptr

  heap_ptr = (uint8_t *)&(((process_data_descriptor_t *)heap_ptr)->names);

  // copy the strings in after the pd
  strcpy((char *)heap_ptr, unit_string);
  heap_ptr += strlen(unit_string)+1;

  strcpy((char *)heap_ptr, name_string);
  heap_ptr += strlen(name_string)+1;

  // moved this up to before the pd record
  /*
  // this aligns the heap pointer to 32bit.  Not doing this causes the floats in the pd to be misaligned, which crashes the arm.
  if((uint32_t)heap_ptr % 4){
     heap_ptr += 4 - (uint32_t)heap_ptr % 4;
  }
  */

  return pd_ptr;
}

uint16_t add_mode(char *name_string, uint8_t index, uint8_t type) {
  mode_descriptor_t mdr = create_mdr(index, type);

  memcpy(heap_ptr, &mdr, sizeof(mode_descriptor_t));

  uint16_t md_ptr = MEMPTR(*heap_ptr);

  heap_ptr = (uint8_t *)&(((mode_descriptor_t *)heap_ptr)->names);

  strcpy((char *)heap_ptr, name_string);
  heap_ptr += strlen(name_string)+1;

  return md_ptr;
}

#define INDIRECT_PD(pd_ptr) ((process_data_descriptor_t *)(memory.bytes + *pd_ptr))
#define DATA_DIR(pd_ptr) INDIRECT_PD(pd_ptr)->data_direction
#define DATA_SIZE(pd_ptr) INDIRECT_PD(pd_ptr)->data_size

#define ADD_PROCESS_VAR(args) *ptocp = add_pd args; input_bits += IS_INPUT(INDIRECT_PD(ptocp)) ? DATA_SIZE(ptocp) : 0; output_bits += IS_OUTPUT(INDIRECT_PD(ptocp)) ? DATA_SIZE(ptocp) : 0; last_pd = INDIRECT_PD(ptocp++)
#define ADD_GLOBAL_VAR(args) *gtocp++ = add_pd args
#define ADD_MODE(args) *gtocp++ = add_mode args

void metadata(pd_metadata_t *pdm, process_data_descriptor_t *ptr) {
  pdm->ptr = ptr;
  pdm->range = ptr->data_type == DATA_TYPE_SIGNED ? MAX(ptr->param_min, ptr->param_max)*2 : ptr->param_max;
  pdm->bitmax = (1<<ptr->data_size)-1;
}

RT_INIT(
  init_hardware();
  rxpos = 0;

  heap_ptr = memory.heap;

  uint16_t input_bits = 8; // this starts at 8 bits = 1 byte for the fault byte
  uint16_t output_bits = 0;

  // these are temp toc arrays that the macros will write pointers into.  the tocs get copied to main memory after everything else is written in
  uint16_t ptoc[32];
  uint16_t gtoc[32]; 

  uint16_t *ptocp = ptoc; uint16_t *gtocp = gtoc;
  process_data_descriptor_t *last_pd;

  ADD_PROCESS_VAR(("output_pins", "none", 2, DATA_TYPE_BITS, DATA_DIRECTION_OUTPUT, 0, 1));       metadata(&(pd_table.output_pins), last_pd);
  ADD_PROCESS_VAR(("pos_cmd", "rad", 16, DATA_TYPE_SIGNED, DATA_DIRECTION_OUTPUT, -3.2, 3.2));        metadata(&(pd_table.pos_cmd), last_pd);
  
  ADD_PROCESS_VAR(("input_pins", "none", 2, DATA_TYPE_BITS, DATA_DIRECTION_INPUT, -100, 100));         metadata(&(pd_table.input_pins), last_pd);
  ADD_PROCESS_VAR(("pos_fb", "rad", 16, DATA_TYPE_SIGNED, DATA_DIRECTION_INPUT, -3.2, 3.2));          metadata(&(pd_table.pos_fb), last_pd);
  //ADD_PROCESS_VAR(("input_pins", "none", 1, DATA_TYPE_BITS, DATA_DIRECTION_INPUT, 0, 1));         metadata(&(pd_table.input_pins), last_pd);

  ADD_GLOBAL_VAR(("swr", "non", 8, DATA_TYPE_UNSIGNED, DATA_DIRECTION_OUTPUT, 0, 0));

  ADD_MODE(("foo", 0, 0));
  ADD_MODE(("io_", 1, 1));

  // automatically create padding pds based on the mod remainder of input/output bits
  if (input_bits % 8)  ADD_PROCESS_VAR(("", "", 8 -  (input_bits%8), DATA_TYPE_PAD, DATA_DIRECTION_INPUT,  0, 0));
  if (output_bits % 8) ADD_PROCESS_VAR(("", "", 8 - (output_bits%8), DATA_TYPE_PAD, DATA_DIRECTION_OUTPUT, 0, 0));

  // now that all the toc entries have been added, write out the tocs to memory and set up the toc pointers

  memory.discovery.input = input_bits >> 3;
  memory.discovery.output = output_bits >> 3;

  memory.discovery.ptocp = MEMPTR(*heap_ptr);

  for(uint8_t i = 0; i < ptocp - ptoc; i++) {
    *heap_ptr++ = ptoc[i] & 0x00FF; 
    *heap_ptr++ = (ptoc[i] & 0xFF00) >> 8;
  }
  *heap_ptr++ = 0x00; *heap_ptr++ = 0x00; // this is the ptoc end marker

  memory.discovery.gtocp = MEMPTR(*heap_ptr);

  for(uint8_t i = 0; i < gtocp - gtoc; i++) {
    *heap_ptr++ = gtoc[i] & 0x00FF; 
    *heap_ptr++ = (gtoc[i] & 0xFF00) >> 8;
  }
  *heap_ptr++ = 0x00; *heap_ptr++ = 0x00; // this is the gtoc end marker


  timeout = 1000;
);

uint8_t crc_reuest(uint8_t len) {
   uint8_t crc = crc8_init();
   for(int i = rxpos; i < rxpos+len; i++){
      crc = crc8_update(crc, (void*)&(rxbuf[i%sizeof(rxbuf)]), 1);  
   }
   crc8_finalize(crc);
   return crc == rxbuf[(rxpos+len)%sizeof(rxbuf)];
}

uint8_t crc8( uint8_t *addr, uint8_t len) {
   uint8_t crc = crc8_init();
   crc = crc8_update(crc, addr, len);
   return crc8_finalize(crc);
}

void send(uint8_t len, uint8_t docrc){
   timeout = 0;
   if(docrc){
      txbuf[len] = crc8((uint8_t *)txbuf,len);
      DMA_SetCurrDataCounter(DMA1_Stream7,len+1);
   }else{
      DMA_SetCurrDataCounter(DMA1_Stream7,len);
   }
   DMA_Cmd(DMA1_Stream7, DISABLE);
   DMA_ClearFlag(DMA1_Stream7, DMA_FLAG_TCIF7);
   DMA_Cmd(DMA1_Stream7, ENABLE);
}

#define BITSLEFT(ptr) (8-ptr)

void process_data_rpc(uint8_t fault, volatile uint8_t *input, volatile uint8_t *output) {
  uint16_t *ptocp = (uint16_t *)(memory.bytes + memory.discovery.ptocp);
  uint32_t local_rxpos = rxpos;
  *(input++) = fault;
  *input = 0x00;

  // data needs to be packed and unpacked based on its type and size
  // input is a pointer to the data that gets sent back to the host
  // need a bit pointer to keep track of partials

  uint8_t output_bit_ptr = 0;
  uint8_t input_bit_ptr = 0;

  while(*ptocp != 0x0000) {
    process_data_descriptor_t *pd = (process_data_descriptor_t *)(memory.bytes + *ptocp++);
    
    if (IS_INPUT(pd)) {
      uint16_t data_addr = pd->data_addr;
      uint8_t data_size = pd->data_size;
      uint8_t data_bit_ptr = 0;
      while(data_size > 0) {
        uint8_t bits_to_pack = data_size < BITSLEFT(input_bit_ptr) ? data_size : BITSLEFT(input_bit_ptr);
        if (BITSLEFT(data_bit_ptr) < bits_to_pack) { bits_to_pack = BITSLEFT(data_bit_ptr); }

        uint8_t mask = ((1<<bits_to_pack) - 1) << (data_bit_ptr);

        *input |= ((MEMU8(data_addr) & mask) >> data_bit_ptr) << input_bit_ptr;

        input_bit_ptr += bits_to_pack;
        data_bit_ptr += bits_to_pack;
        data_size -= bits_to_pack;
        if((input_bit_ptr %= 8) == 0) *(++input) = 0x00; // make sure we clear the input buffer whenever we increment bytes
        if((data_bit_ptr %= 8) == 0) data_addr++;
      }
    }
    if (IS_OUTPUT(pd)) {
      uint16_t data_addr = pd->data_addr;
      uint8_t data_size = pd->data_size;

      uint8_t val_bits_remaining = 8;
      uint8_t val = 0x00;

      while(data_size > 0) {
        // the number of bits to unpack this iteration is the number of bits remaining in the pd, or the number of bits remaining in the output byte, 
        // whichever is smaller.  Then, it can be even smaller if we have less room in the current val.

        uint8_t bits_to_unpack = data_size < BITSLEFT(output_bit_ptr) ? data_size : BITSLEFT(output_bit_ptr);
        if (val_bits_remaining < bits_to_unpack) { bits_to_unpack = val_bits_remaining; }

        // create a bitmask the width of the bits to read, shifted to the position in the output byte that we're pointing to
        uint8_t mask = ((1<<bits_to_unpack) - 1) << (output_bit_ptr);

        // val is what we get when we mask off output and then shift it to the proper place.  
        val = val | ((rxbuf[(local_rxpos+1)%sizeof(rxbuf)] & mask) >> (output_bit_ptr)) << (8-val_bits_remaining); 

        val_bits_remaining -= bits_to_unpack;
        data_size -= bits_to_unpack;
        output_bit_ptr += bits_to_unpack;
        // rxpos is a ringbuf and wraps around
        // note: this replaces the output argument
        if((output_bit_ptr %= 8) == 0){
           local_rxpos++;
           local_rxpos = local_rxpos % sizeof(rxbuf);
        }
        

        if(val_bits_remaining == 0 || data_size == 0) {
          MEMU8(data_addr++) = val;
          val_bits_remaining = 8;
          val = 0x00;
        }
      }
      // now we've finished unpacking it and storing it in memory, but we have to fix up the high bits if it wasn't a byte-aligned datasize.
      // for instance, if we receive 0xFFF in a 12 bit field, that is a negative number, but we stored it as 0x0FFF in memory.
      // strategy is to set the most significant n bits of the MSB to the most significant bit of the output value, iff the pd is defined as signed.
      if (SIGNED(pd) && pd->data_size % 8 != 0) {
        //printf_("in output fixup.  data_addr %h  data_size %i num_bytes %i\n", pd->data_addr, pd->data_size, NUM_BYTES(pd->data_size));
        uint8_t msb_addr = pd->data_addr + NUM_BYTES(pd->data_size) - 1;
        //printf_("in output fixup.  MSB (at %h): %h\n", msb_addr, MEMU8(msb_addr));
        
        // these two masks use data_size%8, this is the number of bits in the most significant byte, and since we tested for %8!=0 above, we know it's a partial byte
        if(MEMU8(msb_addr) & 1<<(pd->data_size%8 - 1)) { // this test uses a mask that is 1 in the most significant bit position, we only need to fixup the val if it's 1 (ie negative)
          // this mask is all the unused high bits set
          uint8_t mask = 0xFF ^ ((1<<pd->data_size%8) - 1);
          //printf_("applying mask: %h\n", mask);
          MEMU8(msb_addr) |= mask;
        }

        //printf_("fixed up val: %h\n", MEMU8(msb_addr));
      }
    }
  }
}


float scale_out(pd_metadata_t pd, int32_t val) {
  return val * pd.range / (float)pd.bitmax;
} 

int32_t scale_in(pd_metadata_t pd, float val) {
  return CLAMP(val, pd.ptr->param_min, pd.ptr->param_max) * pd.bitmax / pd.range;
}


#define BOOLPIN(pin) (uint8_t)(PIN(pin) != 0.0)

RT (
  // update all hal pins with values from their associated pds

  PIN(pos_cmd) = scale_out(pd_table.pos_cmd, (int16_t)(MEMU16(pd_table.pos_cmd.ptr->data_addr)));

  uint8_t outpins = MEMU8(pd_table.output_pins.ptr->data_addr);
  PIN(out0) = outpins & 1 ? 1.0 : 0.0;
  PIN(out1) = outpins & 1 ? 1.0 : 0.0;
  PIN(out2) = outpins & 1 ? 1.0 : 0.0;
  PIN(out3) = outpins & 1 ? 1.0 : 0.0;

  // todo: how to handle bidirectional pins properly?

  *((uint16_t *)&(memory.bytes[pd_table.pos_fb.ptr->data_addr])) = (uint16_t)scale_in(pd_table.pos_fb, PIN(pos_fb));

  MEMU8(pd_table.input_pins.ptr->data_addr) = BOOLPIN(in0)<<0 | BOOLPIN(in1)<<1 | BOOLPIN(in2)<<2 | BOOLPIN(in3)<<3;

);


FRT(
   lbp_t lbp;
   
   char name[] = LBPCardName;
   int bufferpos;
   int available;
   for(int j = 0;j<2;j++){
   //next received packet will be written to bufferpos
   bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA2_Stream5);
   //how many packets we have the the rx buffer for processing
   available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
   
   if (available >= 1) {
      lbp.byte = rxbuf[rxpos];

      if (lbp.ct == CT_LOCAL && lbp.wr == 0) { //local read, cmd+crc = 2b
         if(available >= 2){
            switch(lbp.byte) {
               case LBPCookieCMD:
                 txbuf[0] = LBPCookie;
                 break;
            
               case LBPStatusCMD: //TODO: return status
                 txbuf[0] = 0x00;
                 break;
            
               case LBPCardName0Cmd ... LBPCardName3Cmd:
                 txbuf[0] = name[lbp.byte-LBPCardName0Cmd];
                 break;

               default: //TODO: handle unknown command condition
                 txbuf[0] = 0x00;
            }
            send(1,1);
            rxpos += 2;
         }else{
            continue;
         }
      }else if(lbp.ct == CT_LOCAL && lbp.wr == 1){//local write, cmd+data+crc = 3b
         //0xFF and 0xFC are not followed by crc
         if(rxbuf[rxpos] == 0xFF){
            // reset parser
            timeout = 0;
            rxpos += 1;
         }else if(rxbuf[rxpos] == 0xFC){
            // todo
            timeout = 0;
            rxpos += 1;
         }else if(available >= 3){//writes do not expect crc in reply
            txbuf[0] = 0x00;
            send(1,0);
            rxpos += 3;  
         }else{
            continue;
         }
      }else if(lbp.ct == CT_RPC){//RPC
          if(lbp.byte == UnitNumberRPC && available >= 2){//unit number, cmd+crc = 2b
              txbuf[0] = 0x76;
              txbuf[1] = 0x01;
              txbuf[2] = 0x00;
              txbuf[3] = 0x10;
              send(4,1);
              rxpos += 2;
          }else if(lbp.byte == DiscoveryRPC && available >= 2){//discovery, cmd+crc = 2b
             memcpy((void*)txbuf,((uint8_t*)&memory.discovery),sizeof(memory.discovery));
              send(sizeof(memory.discovery),1);
              rxpos += 2;
          }else if(lbp.byte == ProcessDataRPC && available >= memory.discovery.output + 2){//process data, requires cmd+output bytes+crc
             //TODO: maybe packing and unpacking can be moved to RT
             process_data_rpc(0x00, txbuf, &(rxbuf[rxpos+1])); // todo: send a proper fault byte?
             send(memory.discovery.input,1);
             //we cannot send the reply based on crc, as this causes timeouts
             //instead we should check for errors in RT
             if(!crc_reuest(memory.discovery.output + 1)){
                PIN(crc_error)++;
             }
             rxpos += memory.discovery.output + 2;
          }else{
             continue;
          }
      }else if (lbp.ct == CT_RW && lbp.wr == 0){ //read
         if(available >= 2){
            if (lbp.as == 1){ //address included in command = cmd+addr+addr+crc
               if (available >= 4) {
               address = rxbuf[(rxpos+1)%sizeof(rxbuf)] + (rxbuf[(rxpos+2)%sizeof(rxbuf)]<<8);
               rxpos += 4;
               }else{
                  continue;
               }
            }else{ //address not included in command = cmd+crc
               rxpos += 2;
            }
            memcpy((void*)txbuf,&memory.bytes[address],(1<<lbp.ds));
            send((1<<lbp.ds),1);
            if(lbp.ai == 1){//auto increment address by datasize
               address += (1<<lbp.ds);
            }
         }else{
            continue;
         }
      } else {
         //TODO: handle unkown packet
         PIN(error)++;
      }
   }

   timeout++;
   if(timeout > PIN(timeout)){//TODO: clamping
      PIN(connected) = 0;
      rxpos = bufferpos;
   }else{
      PIN(connected) = 1;
   }
   rxpos = rxpos % sizeof(rxbuf);
}
);


NRT(
  if(PIN(dump_pd_vals) != 0.0) {
    if(PIN(error) != 0) {
      printf_("Error count: %i\n", PIN(error));
    }
    PIN(dump_pd_vals) = 0.0;

    printf_("pos_cmd: %f\n", PIN(pos_cmd));

    uint16_t *ptocp = (uint16_t *)(memory.bytes + memory.discovery.ptocp);
    while(*ptocp != 0x0000) {
      process_data_descriptor_t *pd = (process_data_descriptor_t *)(memory.bytes + *ptocp++);
      printf_("pd has data at %h with value %h\n", pd->data_addr, MEMU16(pd->data_addr));
    }
  }
);

ENDCOMP;