sw/app/rocon/appl_cmdproc.c

   1 #include <system_def.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <stdlib.h>
   5 #include <cmd_proc.h>
   6
   7 #include "appl_defs.h"
   8
   9 typedef struct cmdproc_ready_check_state_t {
  10   unsigned long axes_rq_mask;
  11   int global_rq;
  12   cmd_io_t* peer_cmd_io;
  13 } cmdproc_ready_check_state_t;
  14
  15 #ifndef APPL_WITH_SIM_POSIX
  16 extern cmd_io_t cmd_io_usbcon;
  17 extern cmd_io_t cmd_io_uartcon;
  18
  19 cmdproc_ready_check_state_t cmdproc_ready_check_state_uartcon = {
  20   .peer_cmd_io = &cmd_io_uartcon,
  21 };
  22
  23 cmdproc_ready_check_state_t cmdproc_ready_check_state_usbcon = {
  24   .peer_cmd_io = &cmd_io_usbcon,
  25 };
  26
  27 cmdproc_ready_check_state_t *const cmdproc_ready_check_state_arr[] = {
  28   &cmdproc_ready_check_state_uartcon,
  29   &cmdproc_ready_check_state_usbcon,
  30   NULL
  31 };
  32 #else /*APPL_WITH_SIM_POSIX*/
  33 extern cmd_io_t cmd_io_std_line;
  34
  35 cmdproc_ready_check_state_t cmdproc_ready_check_state_std_line = {
  36   .peer_cmd_io = &cmd_io_std_line,
  37 };
  38
  39 cmdproc_ready_check_state_t *const cmdproc_ready_check_state_arr[] = {
  40   &cmdproc_ready_check_state_std_line,
  41   NULL
  42 };
  43 #endif /*APPL_WITH_SIM_POSIX*/
  44
  45 extern cmd_des_t const *cmd_pxmc_base[];
  46 extern cmd_des_t const *cmd_appl_tests[];
  47 extern cmd_des_t const *cmd_pxmc_ptable[];
  48 extern cmd_des_t const *cmd_pxmc_coordmv[];
  49 extern cmd_des_t const *cmd_appl_specific[];
  50
  51 extern cmd_des_t const cmd_des_dprint;
  52
  53 cmd_des_t const **cmd_list;
  54
  55 cmd_des_t const cmd_des_help =
  56 {
  57   0, 0,
  58   "help", "prints help for commands",
  59   cmd_do_help,
  60   {
  61     (char *) &cmd_list
  62   }
  63 };
  64
  65 cmdproc_ready_check_state_t *cmdproc_ready_state4cmd_io(cmd_io_t *cmd_io)
  66 {
  67   cmdproc_ready_check_state_t * const *ppckst = cmdproc_ready_check_state_arr;
  68   while (*ppckst) {
  69     if ((*ppckst)->peer_cmd_io == cmd_io)
  70       return *ppckst;
  71     ppckst++;
  72   }
  73   return NULL;
  74 }
  75
  76 int cmd_do_r_one(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  77 {
  78   unsigned chan;
  79   unsigned long val;
  80   cmdproc_ready_check_state_t *ckst;
  81
  82   if (*param[2] != ':') return -CMDERR_OPCHAR;
  83   chan = *param[1] - 'A';
  84   if(chan >= 31) return -CMDERR_BADREG;
  85
  86   ckst = cmdproc_ready_state4cmd_io(cmd_io);
  87   if (ckst == NULL)
  88     return -CMDERR_BADDIO;
  89
  90   ckst->axes_rq_mask |= 1l << chan;
  91   return 0;
  92 }
  93
  94 int cmd_do_r_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  95 {
  96   cmdproc_ready_check_state_t *ckst;
  97
  98   if(*param[2] != ':') return -CMDERR_OPCHAR;
  99
 100   ckst = cmdproc_ready_state4cmd_io(cmd_io);
 101   if (ckst == NULL)
 102     return -CMDERR_BADDIO;
 103
 104   ckst->global_rq = 1;
 105   return 0;
 106 }
 107
 108 cmd_des_t const cmd_des_r_one={0, CDESM_OPCHR,
 109                         "R?","send R?! or FAIL?! at axis finish",cmd_do_r_one,{}};
 110
 111 cmd_des_t const cmd_des_r_all={0, CDESM_OPCHR,
 112                         "R","send R! or FAIL! at finish",cmd_do_r_all,{}};
 113
 114 cmd_des_t const *cmd_list_main[] =
 115 {
 116   &cmd_des_help,
 117   CMD_DES_INCLUDE_SUBLIST(cmd_appl_tests),
 118 #ifdef CONFIG_PXMC
 119   CMD_DES_INCLUDE_SUBLIST(cmd_pxmc_base),
 120   CMD_DES_INCLUDE_SUBLIST(cmd_pxmc_ptable),
 121 #endif
 122   &cmd_des_r_one,
 123   &cmd_des_r_all,
 124   CMD_DES_INCLUDE_SUBLIST(cmd_pxmc_coordmv),
 125   CMD_DES_INCLUDE_SUBLIST(cmd_appl_specific),
 126 #ifndef APPL_WITH_SIM_POSIX
 127   &cmd_des_dprint,
 128 #endif /*APPL_WITH_SIM_POSIX*/
 129   NULL
 130 };
 131
 132 cmd_des_t const **cmd_list = cmd_list_main;
 133
 134 int cmdproc_process_ready(cmd_io_t* cmd_io,
 135                   cmdproc_ready_check_state_t *ckst,
 136                   unsigned long busy_bits, unsigned long error_bits)
 137 {
 138   unsigned int chan;
 139   unsigned long send_bits = ckst->axes_rq_mask &
 140                             (error_bits | ~busy_bits);
 141   int send_global = ckst->global_rq && (error_bits || !busy_bits);
 142
 143   if (cmd_io->priv.ed_line.out->inbuf || (!send_bits && !send_global))
 144     return 0;
 145
 146   if (send_bits) {
 147     chan = ffsl(send_bits) - 1;
 148     ckst->axes_rq_mask &= ~(1l << chan);
 149     if (error_bits & (1l << chan))
 150       cmd_io_write(cmd_io,"FAIL",4);
 151     else
 152       cmd_io_write(cmd_io,"R",1);
 153     cmd_io_putc(cmd_io,chan+'A');
 154     cmd_io_putc(cmd_io,'!');
 155     cmd_io_putc(cmd_io,'\r');
 156     cmd_io_putc(cmd_io,'\n');
 157     return 1;
 158   }
 159
 160   ckst->global_rq = 0;
 161   if (error_bits)
 162     cmd_io_write(cmd_io,"FAIL",4);
 163   else
 164     cmd_io_write(cmd_io,"R",1);
 165   cmd_io_putc(cmd_io,'!');
 166   cmd_io_putc(cmd_io,'\r');
 167   cmd_io_putc(cmd_io,'\n');
 168   return 1;
 169 }
 170
 171 int cmdproc_poll(void)
 172 {
 173   static typeof(actual_msec) dbg_prt_last_msec;
 174   static typeof(actual_msec) old_check_time;
 175   typeof(actual_msec) new_check_time;
 176   int ret = 0;
 177   unsigned long busy_bits, error_bits;
 178
 179   lt_mstime_update();
 180
 181   new_check_time = actual_msec;
 182   if (new_check_time != old_check_time) {
 183     cmdproc_ready_check_state_t * const *ppckst;
 184     old_check_time = new_check_time;
 185     pxmc_process_state_check(&busy_bits, &error_bits);
 186     for (ppckst = cmdproc_ready_check_state_arr; *ppckst; ppckst++)
 187       ret |= cmdproc_process_ready((*ppckst)->peer_cmd_io, *ppckst,
 188                                    busy_bits, error_bits);
 189   }
 190
 191  #ifndef APPL_WITH_SIM_POSIX
 192   ret |= cmd_processor_run(&cmd_io_uartcon, cmd_list_main);
 193   ret |= cmd_processor_run(&cmd_io_usbcon, cmd_list_main);
 194
 195   if(!ret && (labs((int)(actual_msec - dbg_prt_last_msec)) > 2000) &&
 196     !cmd_io_uartcon.priv.ed_line.in->lastch) {
 197      cmd_io_t *cmd_io = &cmd_io_uartcon;
 198      dbg_prt_last_msec = actual_msec;
 199
 200      if (cmd_io->priv.ed_line.io_stack)
 201        cmd_io = cmd_io->priv.ed_line.io_stack;
 202      run_dbg_prt(cmd_io);
 203   }
 204  #else /*APPL_WITH_SIM_POSIX*/
 205   ret |= cmd_processor_run(&cmd_io_std_line, cmd_list_main);
 206  #endif /*APPL_WITH_SIM_POSIX*/
 207
 208   return ret;
 209 }