hw/lx-rocon_firmware/firmware.c

   1 /* Firmware file for lx-rocon tumbl coprocessor */
   2
   3 #include <stdint.h>
   4 #include "tumbl_addr.h"
   5
   6 typedef struct pxmcc_axis_data_t {
   7   uint32_t  ptirc;      /* IRC count per phase table */
   8   uint32_t  ptper;      /* number of periods per table */
   9   uint32_t  ptreci;     /* number of periods per table */
  10   uint32_t  ptofs;      /* offset between table and IRC counter */
  11   int32_t   ptsin;
  12   int32_t   ptcos;
  13 } pxmcc_axis_data_t;
  14
  15 uint32_t sin_lat[7];
  16
  17 pxmcc_axis_data_t pxmcc_axis[1];
  18
  19 void init_defvals(void)
  20 {
  21 }
  22
  23 void find_sin_lat(void)
  24 {
  25   int i;
  26   register uint32_t a0, a1, a2, a3, a4, a5;
  27
  28   *FPGA_FNCAPPROX_SIN = 0;
  29
  30   for (i = 0; i < 20; i++)
  31     asm volatile("": : : "memory");
  32
  33   *FPGA_FNCAPPROX_SIN = 0x40000000;
  34   a0 = *FPGA_FNCAPPROX_SIN;
  35   a1 = *FPGA_FNCAPPROX_SIN;
  36   a2 = *FPGA_FNCAPPROX_SIN;
  37   a3 = *FPGA_FNCAPPROX_SIN;
  38   a4 = *FPGA_FNCAPPROX_SIN;
  39   a5 = *FPGA_FNCAPPROX_SIN;
  40   asm volatile("": : : "memory");
  41
  42   sin_lat[0] = 0x1234;
  43   sin_lat[1] = a1;
  44   sin_lat[2] = a2;
  45   sin_lat[3] = a3;
  46   sin_lat[4] = a4;
  47   sin_lat[5] = a5;
  48   sin_lat[6] = 0x4321;
  49 }
  50
  51 void main(void)
  52 {
  53   pxmcc_axis_data_t *pxmcc = pxmcc_axis;
  54
  55   pxmcc->ptirc = *FPGA_IRC0;
  56   pxmcc->ptofs = pxmcc->ptirc;
  57   pxmcc->ptper = 2000;
  58   pxmcc->ptreci = 2147484; /* (1LL<<32)/ptper */
  59
  60   asm volatile("": : : "memory");
  61
  62   find_sin_lat();
  63
  64   while (1) {
  65     uint32_t irc = *FPGA_IRC0;
  66     uint32_t ofs = pxmcc->ptofs;
  67     uint32_t per = pxmcc->ptper;
  68     int32_t  pti;
  69     uint32_t pta;
  70     uint32_t dummy;
  71
  72     pti = irc - ofs;
  73     if ((uint32_t)pti >= per) {
  74       if (pti < 0) {
  75         ofs -= per;
  76       } else {
  77         ofs += per;
  78       }
  79       pti = irc - ofs;
  80       pxmcc->ptofs = ofs;
  81     }
  82     pxmcc->ptirc = pti;
  83
  84     pta = pti * pxmcc->ptreci;
  85
  86     *FPGA_FNCAPPROX_SIN = pta;
  87
  88     dummy = *FPGA_FNCAPPROX_SIN;
  89     dummy = *FPGA_FNCAPPROX_SIN;
  90     dummy = *FPGA_FNCAPPROX_SIN;
  91     pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
  92     dummy = *FPGA_FNCAPPROX_COS;
  93     pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
  94
  95     asm volatile("": : : "memory");
  96   }
  97 }