hm2.c

#include <sys/socket.h>
#include <netinet/in.h>
#include <stdio.h>
#include <string.h>

// https://github.com/LinuxCNC/hostmot2-firmware/blob/v0/regmap#L1105

// 
// idrom addresses & constants
// 

#define HM2_ADDR_IOCOOKIE  (0x0100)
#define HM2_IOCOOKIE       (0x55AACAFE)

#define HM2_ADDR_CONFIGNAME    (0x0104)
#define HM2_CONFIGNAME         "HOSTMOT2"x
#define HM2_CONFIGNAME_LENGTH  (8)

#define HM2_ADDR_IDROM_OFFSET (0x010C)

#define HM2_MAX_MODULE_DESCRIPTORS  (48)
#define HM2_MAX_PIN_DESCRIPTORS     (1000)

// 
// Pin Descriptor constants
// 

#define HM2_PIN_SOURCE_IS_PRIMARY   (0x00000000)
#define HM2_PIN_SOURCE_IS_SECONDARY (0x00000001)

#define HM2_PIN_DIR_IS_INPUT     (0x00000002)
#define HM2_PIN_DIR_IS_OUTPUT    (0x00000004)


// 
// Module Descriptor constants
// 

#define HM2_GTAG_WATCHDOG          (2)
#define HM2_GTAG_IOPORT            (3)
#define HM2_GTAG_ENCODER           (4)
#define HM2_GTAG_STEPGEN           (5)
#define HM2_GTAG_PWMGEN            (6)
#define HM2_GTAG_SPI               (7) // Not supported
#define HM2_GTAG_SSI               (8)
#define HM2_GTAG_UART_TX           (9)
#define HM2_GTAG_UART_RX           (10)
#define HM2_GTAG_TRANSLATIONRAM    (11)
#define HM2_GTAG_MUXED_ENCODER     (12)
#define HM2_GTAG_MUXED_ENCODER_SEL (13)
#define HM2_GTAG_BSPI              (14)
#define HM2_GTAG_DBSPI             (15) // Not supported
#define HM2_GTAG_DPLL              (16) // Not supported
#define HM2_GTAG_MUXED_ENCODER_M   (17) // Not supported
#define HM2_GTAG_MUXED_ENC_SEL_M   (18) // Not supported
#define HM2_GTAG_TPPWM             (19)
#define HM2_GTAG_WAVEGEN           (20) // Not supported
#define HM2_GTAG_DAQFIFO           (21) // Not supported
#define HM2_GTAG_BINOSC            (22) // Not supported
#define HM2_GTAG_DDMA              (23) // Not supported
#define HM2_GTAG_BISS              (24) 
#define HM2_GTAG_FABS              (25) 
#define HM2_GTAG_HM2DPLL           (26) 
#define HM2_GTAG_LIOPORT           (64) // Not supported
#define HM2_GTAG_LED               (128)

#define HM2_GTAG_RESOLVER          (192)
#define HM2_GTAG_SMARTSERIAL       (193)
#define HM2_GTAG_TWIDDLER          (194) // Not supported

typedef struct {
  uint32_t idrom_type;
  uint32_t offset_to_modules;
  uint32_t offset_to_pin_desc;
  uint8_t  board_name[8];  // ascii string, but not NULL terminated!
  uint32_t fpga_size;
  uint32_t fpga_pins;
  uint32_t io_ports;
  uint32_t io_width;
  uint32_t port_width;
  uint32_t clock_low;
  uint32_t clock_high;
  uint32_t instance_stride_0;
  uint32_t instance_stride_1;
  uint32_t register_stride_0;
  uint32_t register_stride_1;
} hm2_idrom_t;

typedef struct {
  // these are from the Pin Descriptor in the HM2 IDROM
  uint8_t sec_pin;//sec pin
  uint8_t sec_tag;//sec func
  uint8_t sec_unit;//sec unit
  uint8_t primary_tag;//Base func
} hm2_pin_t;

typedef struct {//12byte
  uint8_t  gtag;
  uint8_t  version;
  uint8_t  clock_tag;
  uint8_t  instances;
  uint16_t base_address;
  uint8_t  num_registers;
  uint8_t  register_stride:4;//0 or 1, refers to idrom
  uint8_t  instance_stride:4;//0 or 1, refers to idrom
  uint32_t bitmask;
} hm2_module_descriptor_t;

typedef struct {
  uint32_t data;
  uint32_t ddr;
  uint32_t alt_src;
  uint32_t opendrain;
  uint32_t invert;
} ioport_module_t;

/*
cmd   = md->base_address + 0 * 4 + i * 64;
data  = md->base_address + 1 * 4 + i * 64;
cs    = md->base_address + 2 * 4 + i * 64 + c * 4;
addr0 = md->base_address + 3 * 4 + i * 64 + c * 4;
addr1 = md->base_address + 4 * 4 + i * 64 + c * 4;
addr2 = md->base_address + 5 * 4 + i * 64 + c * 4;
*/

#define CHANNELS 2
typedef struct {
  union{
    struct {
      uint8_t ch:8;//channels
      uint8_t t:3;//stop mode
      uint8_t s:1;//stop
      uint8_t d:1;//DoIt
      uint8_t r:1;//request, high for read or write
      uint8_t m:1;//ROM enable/reset
      uint8_t w:1;//write bit
      uint16_t padding;
    } cmd;
    // struct {
    //   uint8_t byte;
    //   uint8_t ch;//channels
    // }cmd;
    uint32_t cmd_word;
  };
  uint32_t padding1[CHANNELS-1];
  uint32_t data;
  uint32_t padding2[CHANNELS-1];
  uint32_t cs[CHANNELS];
  uint32_t u1[CHANNELS];
  uint32_t u2[CHANNELS];
  uint32_t u3[CHANNELS];
  //sserial_module_channel_t ch[7];
} sserial_module_t;

typedef union{
  struct {
    uint8_t n:7;//transfer count
    uint8_t i:1;//increment
    uint8_t s:2;//transfer size
    uint8_t m:3;//memory space identifier
    uint8_t c:1;//space/info
    uint8_t a:1;//address
    uint8_t w:1;//write
  } bits;
  struct {
    uint8_t hi;
    uint8_t lo;
  } bytes;
  uint16_t hilo;
} lbp16_t;

//space 0
typedef union{
  struct{
    hm2_idrom_t idrom;
  };
  struct{
    uint8_t padding3[HM2_ADDR_IOCOOKIE-1];
    uint32_t cookie;
  };
  struct{
    uint8_t padding4[HM2_ADDR_CONFIGNAME-1];
    uint8_t name[HM2_CONFIGNAME_LENGTH];
  };
  struct{
    uint8_t padding5[HM2_ADDR_IDROM_OFFSET-1];
    uint32_t idrom_offset;
    hm2_pin_t pin[34];
    hm2_module_descriptor_t md[3];
    sserial_module_t sserial[1];
    ioport_module_t ioport[2];
  };
  uint8_t bytes[65536];
} lbp_hostmot2_space_t;

//space 2
typedef struct {
  uint16_t reserved1;
  uint8_t  mac[6];
  uint16_t reserved2;
  uint16_t reserved3;
  uint16_t reserved4;
  uint16_t reserved5;
  uint8_t  name[16];
  uint16_t ip_addr_lo;
  uint16_t ip_addr_hi;
  uint16_t reserved6;
  uint16_t reserved7;
  uint16_t led_debug;
  uint16_t reserved8;
  uint16_t reserved9;
  uint16_t reserved10;
} lbp_eth_eeprom_space_t;

//space 4
typedef union{
  uint8_t bytes[65536];
} lbp_timer_util_space_t;

//space 6
typedef union{
  uint8_t bytes[65536];
} lbp_status_control_space_t;

//space 7
typedef struct {
  uint8_t  name[16];
} lbp_card_info_space_t;

void dump_lbp16(lbp16_t lbp16){
  printf("lbp16 cmd: 0x%x\n",lbp16.hilo);
  printf(lbp16.bits.w?"write\n":"read\n");
  // printf(lbp16.bits.a?"address\n":"no address\n");
  // printf(lbp16.bits.c?"info\n":"no info\n");
  // printf("space specifier:%i\n",lbp16.bits.m);
  // printf(lbp16.bits.i?"increment\n":"no increment\n");
}

uint8_t sserial_slave[] = {
  0x05,0x04,0x57,0x00,0x61,0x00,0x00,0x00,
  0x00,0x00,0x00,0x00,0xA0,0x20,0x10,0x80,
  0x00,0x00,0x80,0xFF,0x00,0x00,0x80,0x7F,
  0x08,0x00,0x72,0x61,0x64,0x00,0x70,0x6F,
  0x73,0x5F,0x63,0x6D,0x64,0x00,0x00,0x00,
  0x00,0x00,0x00,0x00,0xA0,0x20,0x10,0x00,
  0x00,0x00,0x80,0xFF,0x00,0x00,0x80,0x7F,
  0x26,0x00,0x72,0x61,0x64,0x00,0x70,0x6F,
  0x73,0x5F,0x66,0x62,0x00,0xB0,0x00,0x01,
  0x00,0x50,0x6F,0x73,0x69,0x74,0x69,0x6F,
  0x6E,0x20,0x6D,0x6F,0x64,0x65,0x00,0x0C,
  0x00,0x2C,0x00,0x01,0x00,0x00,0x00,0x00,
  0x00,0x45,0x00,0x00,0x00,
};
uint16_t sserial_ptocp = 0x0057;
uint16_t sserial_gtocp = 0x0061;

typedef struct {
  float pos_cmd;
} sserial_out_process_data_t; //size:4 bytes

typedef struct {
  float pos_fb;
} sserial_in_process_data_t; //size:4 bytes

  
uint16_t address;
uint8_t cs_read;

void run_sserial(lbp_hostmot2_space_t* hm2){
  //https://github.com/LinuxCNC/linuxcnc/blob/240793be1ea0668f0ad1209e9f19ef5606a2911a/src/hal/drivers/mesa-hostmot2/sserial.c#L149
  for(int i = 0; i < hm2->md[1].instances; i++){
    hm2->sserial[i].cmd_word &= 0x0000FFFF;
    if(hm2->sserial[i].cmd_word != 0){
      //printf("parsing cmd: inst:%i cmd:0x%x data:0x%x\n",i,hm2->sserial[i].cmd_word,hm2->sserial[i].data);
      if(hm2->sserial[i].cmd.m){
        printf("reset\n");
      }else if(hm2->sserial[i].cmd.s){
        printf("start/stop\n");
        if(hm2->sserial[i].cmd.t == 0x00){
          printf("STOP t:0x%u, ch:0x%x\n",hm2->sserial[i].cmd.t,hm2->sserial[i].cmd.ch);
        }else if(hm2->sserial[i].cmd.t == 0x7){
          printf("START\n");
          //TODO: UnitNumberRPC + DiscoveryRPC, LBPCardName0Cmd ... LBPCardName3Cmd
          strncpy((char*)&(hm2->sserial[i].u2[0]), "stbl", 4);
          ((uint16_t*)&(hm2->sserial[i].u3[0]))[0] = sserial_ptocp;
          ((uint16_t*)&(hm2->sserial[i].u3[0]))[1] = sserial_gtocp;
          
          strncpy((char*)&(hm2->sserial[i].u2[1]), "stbl", 4);
          ((uint16_t*)&(hm2->sserial[i].u3[1]))[0] = sserial_ptocp;
          ((uint16_t*)&(hm2->sserial[i].u3[1]))[1] = sserial_gtocp;
        }
      }else if(hm2->sserial[i].cmd.r){
        //TODO: address...
        hm2->sserial[i].data = 0x00;
        printf("get sw version\n");
      }else if(hm2->sserial[i].cmd.d){//do it
        if(cs_read){
          // printf("DO IT channel 0 cs read addr:%u data:%u\n",address,sserial_slave[address]);
          //TODO: read from slave, 1 channel only
          ((uint8_t*)&(hm2->sserial[i].u1[0]))[0] = sserial_slave[address];
          ((uint8_t*)&(hm2->sserial[i].u1[1]))[0] = sserial_slave[address];
          cs_read = 0;
        }else{
          // printf("DO IT channel 0, u1:%x,u2:%x,u3:%x\n",hm2->sserial[i].u1[0],hm2->sserial[i].u2[0],hm2->sserial[i].u3[0]);
          //TODO: ProcessDataRPC
          sserial_out_process_data_t pd_out;
          sserial_in_process_data_t pd_in;
          float x,y;
          memcpy(&pd_out,&hm2->sserial[i].u1[0],sizeof(pd_out));
          pd_in.pos_fb = pd_out.pos_cmd + 1.0;
          x = pd_out.pos_cmd;
          memcpy(&hm2->sserial[i].u1[0],&pd_in,sizeof(pd_in));
          
          memcpy(&pd_out,&hm2->sserial[i].u1[1],sizeof(pd_out));
          pd_in.pos_fb = pd_out.pos_cmd + 1.0;
          y = pd_out.pos_cmd;
          memcpy(&hm2->sserial[i].u1[1],&pd_in,sizeof(pd_in));
          // printf("pos_cmd:%f,%f\n",x,y);
        }
      }else{
        printf("unknown sserial cmd: inst:%i cmd:0x%x data:0x%x\n",i,hm2->sserial[i].cmd_word,hm2->sserial[i].data);
        //printf("sserial inst:%i cmd:%u data:%u\n",i,hm2->sserial[i].cmd_word,hm2->sserial[i].data);
      }
      //ack command
      hm2->sserial[i].cmd_word = 0x00;
      //clear data, TODO: breaks local reads
      hm2->sserial[i].data = 0x00;   
    }
    if((hm2->sserial[i].cs[0] >> 24) == 0x4c){
      printf("CS do address %u\n",address);
      address = hm2->sserial[i].cs[0];
      hm2->sserial[i].cs[0] = 0x00;
      cs_read = 1;
    }
    if((hm2->sserial[i].cs[1] >> 24) == 0x4c){
      printf("CS do address %u\n",address);
      address = hm2->sserial[i].cs[1];
      hm2->sserial[i].cs[1] = 0x00;
      cs_read = 1;
    }
  }//end instance loop
}//run_sserial

int main(int argc, char**argv)
{
  int sockfd;
  struct sockaddr_in servaddr,cliaddr;
  socklen_t len;
  sockfd=socket(AF_INET,SOCK_DGRAM,0);

  servaddr.sin_family = AF_INET;
  servaddr.sin_addr.s_addr=htonl(INADDR_ANY);
  servaddr.sin_port=htons(27181);
  bind(sockfd,(struct sockaddr *)&servaddr,sizeof(servaddr));
     
  //memory spaces
  lbp_hostmot2_space_t lbp_hostmot2_space;//space 0
  lbp_eth_eeprom_space_t lbp_eth_eeprom_space;//space 2
  lbp_timer_util_space_t lbp_timer_util_space;//space 4
  lbp_status_control_space_t lbp_status_control_space; //space 6
  lbp_card_info_space_t lbp_card_info_space;//space 7
  void* space[8];
  //memory spaces:
  //0: HostMot2 space 64k
  //1: Ethernet chip
  //2: Ethernet EEPROM 128 bytes
  //3: FPGA flash 12 bytes
  //4: timers 30 byte
  //5:
  //6: LBP16 control/status 30 byte
  //7: LBP16 info 30 byte
  
  space[0] = &lbp_hostmot2_space;
   
  space[1] = &lbp_eth_eeprom_space;
  space[3] = &lbp_eth_eeprom_space;
  space[5] = &lbp_eth_eeprom_space;
   
  space[2] = &lbp_eth_eeprom_space;
  space[4] = &lbp_timer_util_space;
  space[6] = &lbp_status_control_space;
  space[7] = &lbp_card_info_space;
   
  //hm2 reads:
  //1. mac address
  //2. name
  //3. cookie, 0x55aacafe
  //4. Config name, HOSTMOT2
  //5. idrom offset
  //6. idrom type
  //7. complete idrom
  //8. pins
  //9. module descriptors
  
  lbp_eth_eeprom_space.mac[0] = 0x7e;
  lbp_eth_eeprom_space.mac[1] = 0x4f;
  lbp_eth_eeprom_space.mac[2] = 0x7e;
  lbp_eth_eeprom_space.mac[3] = 0x80;
  lbp_eth_eeprom_space.mac[4] = 0xde;
  lbp_eth_eeprom_space.mac[5] = 0x94;

  strcpy((char*)&(lbp_card_info_space.name[0]), "7I92");

  lbp_hostmot2_space.cookie = HM2_IOCOOKIE;
  lbp_hostmot2_space.idrom_offset = 0x000000;

  strncpy((char*)&(lbp_hostmot2_space.bytes[260]), "HOSTMOT2", 8);
   
  lbp_hostmot2_space.idrom.idrom_type = 3;
  lbp_hostmot2_space.idrom.port_width = 17;
  lbp_hostmot2_space.idrom.io_ports = 2;
  lbp_hostmot2_space.idrom.io_width = 34;
  lbp_hostmot2_space.idrom.clock_low = 5000000;
  lbp_hostmot2_space.idrom.clock_high = 5000000;
  lbp_hostmot2_space.idrom.offset_to_pin_desc = (uint32_t)&lbp_hostmot2_space.pin-(uint32_t)&lbp_hostmot2_space;
  lbp_hostmot2_space.idrom.offset_to_modules = (uint32_t)&lbp_hostmot2_space.md-(uint32_t)&lbp_hostmot2_space;
  lbp_hostmot2_space.idrom.instance_stride_0 = 0x00000004;
  lbp_hostmot2_space.idrom.register_stride_0 = 0x00000004;
  lbp_hostmot2_space.idrom.instance_stride_1 = 0x00000040;
  lbp_hostmot2_space.idrom.register_stride_1 = CHANNELS * 4;//depends on number of channels...
   
  lbp_hostmot2_space.md[0].gtag = HM2_GTAG_IOPORT;
  lbp_hostmot2_space.md[0].version = 0;
  lbp_hostmot2_space.md[0].clock_tag = 0x01;
  lbp_hostmot2_space.md[0].instances = 0x02;
  lbp_hostmot2_space.md[0].base_address = (uint32_t)&lbp_hostmot2_space.ioport[0]-(uint32_t)&lbp_hostmot2_space;
  lbp_hostmot2_space.md[0].num_registers = 0x05;
  lbp_hostmot2_space.md[0].register_stride = 0x00;
  lbp_hostmot2_space.md[0].instance_stride = 0x00;
  lbp_hostmot2_space.md[0].bitmask = 0x0000001F;

  lbp_hostmot2_space.md[1].gtag = HM2_GTAG_SMARTSERIAL;
  lbp_hostmot2_space.md[1].version = 0;
  lbp_hostmot2_space.md[1].clock_tag = 0x01;
  lbp_hostmot2_space.md[1].instances = 0x01;
  lbp_hostmot2_space.md[1].base_address = (uint32_t)&lbp_hostmot2_space.sserial[0]-(uint32_t)&lbp_hostmot2_space;
  lbp_hostmot2_space.md[1].num_registers = 6;//forced by driver
  lbp_hostmot2_space.md[1].register_stride = 0x01;
  lbp_hostmot2_space.md[1].instance_stride = 0x01;//forced to be 0x40 = 64
  lbp_hostmot2_space.md[1].bitmask = 0x0000003C;//forced by driver, multiple registers

  lbp_hostmot2_space.md[2].gtag = 0x00;//no more modules
   
  for(int i = 0;i<lbp_hostmot2_space.idrom.io_width;i++){
    lbp_hostmot2_space.pin[i].primary_tag = HM2_GTAG_IOPORT;
    lbp_hostmot2_space.pin[i].sec_pin = 0;
    lbp_hostmot2_space.pin[i].sec_unit = 0;
    lbp_hostmot2_space.pin[i].sec_tag = 0;
  }
  for(int i = 0;i <= CHANNELS; i+=2){
    lbp_hostmot2_space.pin[i].sec_tag = HM2_GTAG_SMARTSERIAL;
    lbp_hostmot2_space.pin[i].sec_unit = 0;
    lbp_hostmot2_space.pin[i].sec_pin = 0x01 + i/2;
    lbp_hostmot2_space.pin[i+1].sec_tag = HM2_GTAG_SMARTSERIAL;
    lbp_hostmot2_space.pin[i+1].sec_unit = 0;
    lbp_hostmot2_space.pin[i+1].sec_pin = 0x81 + i/2;
  }
  
  
  lbp16_t lbp16;
  //TODO: Each memory space has its own address pointer.
  uint16_t address = 0;//address pointer
  unsigned int transfer_size;
  uint8_t txbuf[1000];
  uint8_t rxbuf[1000];
  int txlen = 0;
  int rxlen = 0;

  printf("CM: 0x%x\n",(uint32_t)&lbp_hostmot2_space.sserial[0].cmd_word - (uint32_t)&lbp_hostmot2_space);
  printf("CS: 0x%x\n",(uint32_t)&lbp_hostmot2_space.sserial[0].cs[0] - (uint32_t)&lbp_hostmot2_space);
  printf("U0: 0x%x\n",(uint32_t)&lbp_hostmot2_space.sserial[0].u1[0] - (uint32_t)&lbp_hostmot2_space);
  printf("U1: 0x%x\n",(uint32_t)&lbp_hostmot2_space.sserial[0].u2[0] - (uint32_t)&lbp_hostmot2_space);
  printf("U2: 0x%x\n",(uint32_t)&lbp_hostmot2_space.sserial[0].u3[0] - (uint32_t)&lbp_hostmot2_space);

  for (;;)
  {
    len = sizeof(cliaddr);
    rxlen = recvfrom(sockfd,rxbuf,sizeof(rxbuf),0,(struct sockaddr *)&cliaddr,&len);
    rxbuf[rxlen] = 0;

    int pos = 0;
    if(rxlen%2!=0){
      printf("uneven packet size\n");
    }
    printf("################\n");
    printf("start of packet len %i\n",rxlen);
    while(pos < rxlen){
      int datastart = 2;
      lbp16.bytes.hi = rxbuf[pos+0];
      lbp16.bytes.lo = rxbuf[pos+1];
      transfer_size = (1 << lbp16.bits.s) * lbp16.bits.n;
      if(lbp16.bits.a == 1){
        address = rxbuf[pos+2] + (rxbuf[pos+3] << 8);
        datastart = 4;
      }
      dump_lbp16(lbp16);
      printf("addr: 0x%x len: %i\n",address,transfer_size);
      if(lbp16.bits.w){//write
        if(transfer_size <= rxlen - datastart - pos){
          for(int i = 0;i<transfer_size;i++){
            *(uint8_t*)&(space[lbp16.bits.m][address+i]) = rxbuf[datastart+i+pos];
          }
          address += transfer_size;
          pos += datastart+transfer_size;
        }else{
          printf("incomplete packet\n");
        }
      }else{//read
        //TODO: check lots of stuff
        for(int i = 0;i<transfer_size;i++){
          txbuf[txlen++] = ((uint8_t*)space[lbp16.bits.m])[i+address];
        }
        address += transfer_size;
        pos += datastart;
      } 
    }
    run_sserial(&lbp_hostmot2_space);
    if(txlen > 0){
      sendto(sockfd,txbuf,txlen,0,(struct sockaddr *)&cliaddr,sizeof(cliaddr));
      printf("sending %i bytes\n",txlen);
      txlen = 0;
    }
    printf("end of packet\n");
  }
}