stmbl

#include "commands.h"
#include "commandslist.h"

HAL_COMP(term);

#define TERM_NUM_WAVES 8

HAL_PIN_ARRAY(wave, TERM_NUM_WAVES);
HAL_PIN_ARRAY(gain, TERM_NUM_WAVES);
HAL_PIN_ARRAY(offset, TERM_NUM_WAVES);

HAL_PIN(send_step) = 10.0;

HAL_PIN(buf_use) = 0.0;

HAL_PIN(con) = 0.0;
HAL_PIN(jogvel) = 10.0;
HAL_PIN(jogpos) = 0.0;
HAL_PIN(jogging) = 0.0;

HAL_PIN(test) = 0.0;

#define TERM_BUF_SIZE 24

MEM(float wave_buf[TERM_BUF_SIZE][TERM_NUM_WAVES]);

MEM(unsigned int read_pos) = 0;
MEM(unsigned int write_pos) = 0;
MEM(unsigned int send_counter) = 0;

MEM(volatile float jog_timeout) = 0.0;
MEM(volatile float jog_en) = 0.0;

INIT(
   if(hal_conf_load()){//flash load error
      hal.hal_state = CONFIG_LOAD_ERROR;
   }
);

RT(
   if(jog_timeout < 0.5){
      jog_timeout += period;
      PIN(jogpos) += jog_en * PIN(jogvel) * period;
      PIN(jogpos) = mod(PIN(jogpos));
   }else{
      PIN(jogging) = 0.0;
   }

   if(send_counter++ >= PIN(send_step) - 1){
      for(int i = 0; i < TERM_NUM_WAVES; i++){
         wave_buf[write_pos][i] = PIN_ARRAY(wave,i);
      }

      write_pos++;
      write_pos %= TERM_BUF_SIZE;
      send_counter = 0;
   }
);

NRT(
   
   void print_version_info(const volatile struct version_info* v){
      printf(
          "%s v%i.%i.%i %s\n",
          v->product_name,
          v->major,
          v->minor,
          v->patch,
          v->git_version
      );
      printf("Branch %s\n",v->git_branch);
      printf("Compiled %s %s ",v->build_date, v->build_time);
      printf("by %s on %s\n",v->build_user, v->build_host);
   }

   void about(){
      
      //hv firmware. defined by the linker script
      // extern const char _binary_obj_hv_hv_bin_start;
      // extern const char _binary_obj_hv_hv_bin_size;
      // extern const char _binary_obj_hv_hv_bin_end;

      //version info of bootloader
      volatile const struct version_info* bt_version_info = (void*)0x08000188;
      //version info of embedded hv firmware
      // volatile const struct version_info* hv_version_info = (void*)(&_binary_obj_hv_hv_bin_start + 0x10c);
      
      printf("######## software info ########\n");
      print_version_info(&version_info);
      printf("GCC        %s\n",__VERSION__);
      printf("newlib     %s\n",_NEWLIB_VERSION);
      printf("CMSIS      %i.%i\n",__CM4_CMSIS_VERSION_MAIN,__CM4_CMSIS_VERSION_SUB);
      printf("StdPeriph  %i.%i.%i\n",__STM32F4XX_STDPERIPH_VERSION_MAIN,__STM32F4XX_STDPERIPH_VERSION_SUB1,__STM32F4XX_STDPERIPH_VERSION_SUB2);
      printf("CPU ID     %lx %lx %lx\n",U_ID[0], U_ID[1], U_ID[2]);
      printf("Unit no    %lx\n",U_ID[0] ^ U_ID[1] ^ U_ID[2]);
      CRC_ResetDR();
      uint32_t crc = CRC_CalcBlockCRC((uint32_t *) 0x08010000, version_info.image_size / 4);
      printf("size: %lu crc:%lx\n", version_info.image_size, version_info.image_crc);
      if(crc == 0)
         printf("crc ok!\n");
      else
         printf("crc error!:%lx\n", crc);
      
      printf("######## Bootloader info ########\n");
      print_version_info(bt_version_info);
      
      // printf("######## HV info ########\n");
      // printf("hv: start:%p ,size:%p ,end%p \n",&_binary_obj_hv_hv_bin_start,&_binary_obj_hv_hv_bin_size,&_binary_obj_hv_hv_bin_end);
      // print_version_info(hv_version_info);
   }

   void sysinfo(){
      printf("######## sysinfo ########\n");
      extern char _etext;    // end address of the .text section
      extern char _sidata;   // start address of the initialization values of the .data section
      extern char _sdata;    // start address of the .data section
      extern char _edata;    // end address of the .data section
      extern char _sbss;     // start address of the .bss section
      extern char _ebss;     // end address of the .bss section
      //extern char _snoinit;  // start address of the .noinit section
      //extern char _enoinit;  // end address of the .noinit section
      extern char _end;      // end address of the .bss section
      extern char _estack;   // initial value of the stack pointer

      extern char *__brkval;

      RCC_ClocksTypeDef  RCC_ClocksStatus;
      RCC_GetClocksFreq(&RCC_ClocksStatus);

      printf("HSE_VALUE        = %uHz\n", HSE_VALUE);
      printf("SYSCLK_Frequency = %luHz\n", RCC_ClocksStatus.SYSCLK_Frequency );
      printf("HCLK_Frequency   = %luHz\n", RCC_ClocksStatus.HCLK_Frequency   );
      printf("PCLK1_Frequency  = %luHz\n", RCC_ClocksStatus.PCLK1_Frequency  );
      printf("PCLK2_Frequency  = %luHz\n", RCC_ClocksStatus.PCLK2_Frequency  );
      extern void *g_pfnVectors;
      printf("vtor %lu\n",(uint32_t)&g_pfnVectors);
      //*****************************************************************
      printf("RCC->CSR = %x\n", (unsigned int)RCC->CSR);
      printf("_etext   = %p\n", &_etext  );
      printf("_sidata  = %p\n", &_sidata );
      printf("_sdata   = %p\n", &_sdata  );
      printf("_edata   = %p\n", &_edata  );
      printf("_sbss    = %p\n", &_sbss   );
      printf("_ebss    = %p\n", &_ebss   );
      // printf("_snoinit = %p\n", &_snoinit);
      // printf("_enoinit = %p\n", &_enoinit);
      printf("_end     = %p\n", &_end    );
      printf("_estack  = %p\n", &_estack );

      printf("heap avail       = %luB\n", (uint32_t)((char*)__get_MSP() - __brkval));
      uint32_t fpscr = get_fpscr();
      printf("FPU fpscr = %x\n", (unsigned int)fpscr);
      printf(" IDC: %i IXC: %i UFC: %i OFC: %i DZC: %i IOC %i\n", (fpscr & (1 << 7)) > 0, (fpscr & (1 << 4)) > 0, (fpscr & (1 << 3)) > 0, (fpscr & (1 << 2)) > 0, (fpscr & (1 << 1)) > 0, (fpscr & (1 << 0)) > 0);
      fpscr &= (uint32_t)~0x9F; // Clear all exception flags
      set_fpscr(fpscr);
   }
   void hal_term_nan(){
      PIN(test) = sqrtf(-1.0);
   }
   void hal_term_over(){ // TODO
      PIN(test) = INFINITY + 1.0;
   }
   void hal_term_under(){ // TODO
      PIN(test) = 1.0 / INFINITY;
   }
   void hal_term_zero(){
      PIN(test) = 1.0 / 0.0;
   }

   void help(){
      printf("######## HAL cheat sheet ########\n");
      printf("pin name: <comp name><comp instance number>.<pin name> (e.g. pid0.enable)\n");
      printf("show pin value and source: pid0.enable (answer: pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf("set pin: pid0.enable = 1.5 (answer: OK pid0.enable = 1.500000)\n");
      printf("link pin: pid0.enable = fault0.enable_pid (answer: OK pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf("unlink pin: pid0.enable = pid0.enable (answer: OK pid0.enable <= pid0.enable = 1.000000)\n");
      printf("show hal info: hal\n");
      printf("show sys info: sysinfo\n");
      printf("show software version: about\n");
      printf("stop realtime hal: stop\n");
      printf("start realtime hal: start\n");
      printf("change rt priority: pid0.rt_prio = 6 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf("change frt priority: pid0.frt_prio = -1 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf("reset cpu: reset\n");
      printf("print help: help\n");
      printf("list all hal pins: list\n");
      printf("list all conf pins: getconf\n");
      printf("save conf: save\n");
      printf("load conf: load\n");
   }
   
   uint32_t call_cmd(char * s, char * args){
      for(uint32_t i = 0; i < sizeof(cmd) / sizeof(cmd_t); i++){
         if(!strcmp(cmd[i].name, s)){
            cmd[i].ptr(args);
            return(1);
         }
      }
      return(0);
   }

   void cmd(char * s){
      if(call_cmd(s, "")){
            
      }
      else if(!strcmp(s, "bootloader")){//reset and jump to bootloader
         *((unsigned long *)0x2001C000) = 0xDEADBEEF;//set bootloader trigger
         NVIC_SystemReset();
      }
      else if(!strcmp(s, "start")){
         printf("starting hal ... ");
         hal_start();
         printf("done\n");
      }
      else if(!strcmp(s, "stop")){
         printf("stopping hal ... ");
         hal_stop();
         printf("done\n");
      }
      else if(!strcmp(s, "test_nan")){
         hal_term_nan();
      }
      else if(!strcmp(s, "test_overflow")){
         hal_term_over();
      }
      else if(!strcmp(s, "test_underflow")){
         hal_term_under();
      }
      else if(!strcmp(s, "test_div_zero")){
         hal_term_zero();
      }
      else if(!strcmp(s, "hal")){
         hal_term_print_info();
      }
      else if(!strcmp(s, "about")){
         about();
      }
      else if(!strcmp(s, "sysinfo")){
         sysinfo();
      }
      else if(!strcmp(s, "reset")){
         NVIC_SystemReset();
      }
      else if(!strcmp(s, "list")){
         hal_term_list();
      }
      else if(!strcmp(s, "help")){
         help();
      }
      else if(!strcmp(s, "save")){
         hal_conf_save();
      }
      else if(!strcmp(s, "confdiff")){
         hal_conf_diff();
      }
      else if(!strcmp(s, "load")){
         int ret = hal_conf_load();
         if(ret == -1){
            printf("flash error\n");
            return;
         }else if(ret == -2){
            printf("NB_OF_VAR too small\n");
            return;
         }else if(ret == -3){
            printf("flash read error\n");
         }else if(ret == -4){
            printf("cannot load while hal is running(run 'stop' command)\n");
         }else if(ret == -5){
            printf("CRC error\n");
         }else{
            printf("done\n");
         }
      }
      else if(!strcmp(s, "initflash")){//TODO: command to format param sectors, init sometimes fails...
         if(hal.rt_state != RT_STOP || hal.frt_state != FRT_STOP){
            printf("cannot init flash while hal is running(run 'stop' command)\n");
         }else{
            uint16_t ret = hal_conf_init();
            if(ret == FLASH_COMPLETE){
               printf("flash init OK. now do load or save\n");
            }else{
               printf("init error: %i\n",ret);  
            }
         }
      }
      else if(!strcmp(s, "getconf")){
         hal_term_getconf();
      }
      else if(!strcmp(s, "jogl")){
         jog_en = -1.0;
         jog_timeout = 0.0;
         PIN(jogging) = 1.0;
      }
      else if(!strcmp(s, "jogr")){
         jog_en = 1.0;
         jog_timeout = 0.0;
         PIN(jogging) = 1.0;
      }
      else if(!strcmp(s, "jogx")){
         jog_en = 0.0;
         PIN(jogging) = 0.0;
      }
      else{
         printf("not found: %s\n",s);
      }
   }

   int tmp = 0;
   unsigned char buf[8 + 2];//TODO: TERM_NUM_WAVES...

   buf[0] = 255;
   buf[8 + 1] = 0;

   unsigned int wp = write_pos;
   unsigned int bc = 0;

   while(read_pos != wp){
      bc++;
      
      for(int i = 0; i < TERM_NUM_WAVES; i++){
         tmp = (wave_buf[read_pos][i] + PIN_ARRAY(offset,i)) * PIN_ARRAY(gain,i) + 128;
         buf[i+1] = CLAMP(tmp,1,254);
      }

      read_pos++;
      read_pos %= TERM_BUF_SIZE;

      buf[8 + 1] = 0;

      if (USB_CDC_is_connected() && PIN(send_step) > 0) {
         PIN(con) = 1.0;
         USB_VCP_send_string(buf);
      }else{
         PIN(con) = 0.0;
      }
   }

   PIN(buf_use) = bc;

   if (USB_CDC_is_connected() && systime >= 1000) {
      char source[APP_TX_BUF_SIZE];
      char sink[APP_TX_BUF_SIZE];

      if(EDGE(hal.hal_state)){
         hal_term_print_state();
      }

      int i = -1;
      char rx_buf[APP_TX_BUF_SIZE];
      if (USB_VCP_get_string(rx_buf)) {
         i = sscanf_(rx_buf, "%N = %N", sink, source);//TODO: change to lib sscanf
      }
      if(i == 2){ // read hal pin
         if(hal_is_pin(sink)){
            hal_term_print_pin(hal_find_pin(sink));
         }
         else if(hal_is_compname(sink)){
            for(int i = 0; i < hal.hal_pin_count; i++){//find all pins of this comp
              if(!strncmp(hal.hal_pins[i]->name, sink, strlen(sink))){
                 hal_term_print_pin(hal.hal_pins[i]);
              }
            }
         }else{
            cmd(rx_buf);
         }
      }
      else if(i == 5){// link hal pin
         if(hal_is_pin(source) && hal_is_pin(sink)){
            hal_link_pins(source, sink);
            printf("OK %s <= %s = %f\n", sink, source, hal_get_pin(sink));
         }
         else if(hal_is_pin(sink)){// set hal pin
            hal_set_pin(sink, strtof(source,NULL));
            printf("OK %s = %f\n", sink, hal_get_pin(sink));
         }
         else{// hal pin not found
            printf("not found: %s\n", sink);
         }
      }
   }
);

ENDCOMP;