git.gag.com Git - fw/altos/blob - src/core/ao_cmd.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include "ao.h"
  19
  20 __pdata uint16_t ao_cmd_lex_i;
  21 __pdata uint32_t ao_cmd_lex_u32;
  22 __pdata char    ao_cmd_lex_c;
  23 __pdata enum ao_cmd_status ao_cmd_status;
  24
  25 #define CMD_LEN 48
  26
  27 static __xdata char     cmd_line[CMD_LEN];
  28 static __pdata uint8_t  cmd_len;
  29 static __pdata uint8_t  cmd_i;
  30
  31 void
  32 ao_put_string(__code char *s)
  33 {
  34         char    c;
  35         while ((c = *s++))
  36                 putchar(c);
  37 }
  38
  39 static void
  40 backspace(void)
  41 {
  42         ao_put_string ("\010 \010");
  43 }
  44
  45 static void
  46 readline(void)
  47 {
  48         char c;
  49         if (ao_echo())
  50                 ao_put_string("> ");
  51         cmd_len = 0;
  52         for (;;) {
  53                 flush();
  54                 c = getchar();
  55                 /* backspace/delete */
  56                 if (c == '\010' || c == '\177') {
  57                         if (cmd_len != 0) {
  58                                 if (ao_echo())
  59                                         backspace();
  60                                 --cmd_len;
  61                         }
  62                         continue;
  63                 }
  64
  65                 /* ^U */
  66                 if (c == '\025') {
  67                         while (cmd_len != 0) {
  68                                 if (ao_echo())
  69                                         backspace();
  70                                 --cmd_len;
  71                         }
  72                         continue;
  73                 }
  74
  75                 /* map CR to NL */
  76                 if (c == '\r')
  77                         c = '\n';
  78
  79                 if (c == '\n') {
  80                         if (ao_echo())
  81                                 putchar('\n');
  82                         break;
  83                 }
  84
  85                 if (cmd_len >= CMD_LEN - 2)
  86                         continue;
  87                 cmd_line[cmd_len++] = c;
  88                 if (ao_echo())
  89                         putchar(c);
  90         }
  91         cmd_line[cmd_len++] = '\n';
  92         cmd_line[cmd_len++] = '\0';
  93         cmd_i = 0;
  94 }
  95
  96 void
  97 ao_cmd_lex(void)
  98 {
  99         ao_cmd_lex_c = '\n';
 100         if (cmd_i < cmd_len)
 101                 ao_cmd_lex_c = cmd_line[cmd_i++];
 102 }
 103
 104 static void
 105 putnibble(uint8_t v)
 106 {
 107         if (v < 10)
 108                 putchar(v + '0');
 109         else
 110                 putchar(v + ('a' - 10));
 111 }
 112
 113 uint8_t
 114 ao_getnibble(void)
 115 {
 116         char    c;
 117
 118         c = getchar();
 119         if ('0' <= c && c <= '9')
 120                 return c - '0';
 121         if ('a' <= c && c <= 'f')
 122                 return c - ('a' - 10);
 123         if ('A' <= c && c <= 'F')
 124                 return c - ('A' - 10);
 125         ao_cmd_status = ao_cmd_lex_error;
 126         return 0;
 127 }
 128
 129 void
 130 ao_cmd_put16(uint16_t v)
 131 {
 132         ao_cmd_put8(v >> 8);
 133         ao_cmd_put8(v);
 134 }
 135
 136 void
 137 ao_cmd_put8(uint8_t v)
 138 {
 139         putnibble((v >> 4) & 0xf);
 140         putnibble(v & 0xf);
 141 }
 142
 143 uint8_t
 144 ao_cmd_is_white(void)
 145 {
 146         return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t';
 147 }
 148
 149 void
 150 ao_cmd_white(void)
 151 {
 152         while (ao_cmd_is_white())
 153                 ao_cmd_lex();
 154 }
 155
 156 int8_t
 157 ao_cmd_hexchar(char c)
 158 {
 159         if ('0' <= c && c <= '9')
 160                 return (c - '0');
 161         if ('a' <= c && c <= 'f')
 162                 return (c - 'a' + 10);
 163         if ('A' <= c && c <= 'F')
 164                 return (c - 'A' + 10);
 165         return -1;
 166 }
 167
 168 void
 169 ao_cmd_hexbyte(void)
 170 {
 171         uint8_t i;
 172         int8_t  n;
 173
 174         ao_cmd_lex_i = 0;
 175         ao_cmd_white();
 176         for (i = 0; i < 2; i++) {
 177                 n = ao_cmd_hexchar(ao_cmd_lex_c);
 178                 if (n < 0) {
 179                         ao_cmd_status = ao_cmd_syntax_error;
 180                         break;
 181                 }
 182                 ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
 183                 ao_cmd_lex();
 184         }
 185 }
 186
 187 void
 188 ao_cmd_hex(void)
 189 {
 190         __pdata uint8_t r = ao_cmd_lex_error;
 191         int8_t  n;
 192
 193         ao_cmd_lex_i = 0;
 194         ao_cmd_white();
 195         for(;;) {
 196                 n = ao_cmd_hexchar(ao_cmd_lex_c);
 197                 if (n < 0)
 198                         break;
 199                 ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
 200                 r = ao_cmd_success;
 201                 ao_cmd_lex();
 202         }
 203         if (r != ao_cmd_success)
 204                 ao_cmd_status = r;
 205 }
 206
 207 void
 208 ao_cmd_decimal(void)
 209 {
 210         __pdata uint8_t r = ao_cmd_lex_error;
 211
 212         ao_cmd_lex_u32 = 0;
 213         ao_cmd_white();
 214         for(;;) {
 215                 if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9')
 216                         ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0');
 217                 else
 218                         break;
 219                 r = ao_cmd_success;
 220                 ao_cmd_lex();
 221         }
 222         if (r != ao_cmd_success)
 223                 ao_cmd_status = r;
 224         ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32;
 225 }
 226
 227 uint8_t
 228 ao_match_word(__code char *word)
 229 {
 230         while (*word) {
 231                 if (ao_cmd_lex_c != *word) {
 232                         ao_cmd_status = ao_cmd_syntax_error;
 233                         return 0;
 234                 }
 235                 word++;
 236                 ao_cmd_lex();
 237         }
 238         return 1;
 239 }
 240
 241 static void
 242 echo(void)
 243 {
 244         ao_cmd_hex();
 245         if (ao_cmd_status == ao_cmd_success)
 246                 ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0;
 247 }
 248
 249 static void
 250 ao_reboot(void)
 251 {
 252         ao_cmd_white();
 253         if (!ao_match_word("eboot"))
 254                 return;
 255         /* Delay waiting for the packet master to be turned off
 256          * so that we don't end up back in idle mode because we
 257          * received a packet after boot.
 258          */
 259         flush();
 260         ao_delay(AO_SEC_TO_TICKS(1));
 261         ao_arch_reboot();
 262         ao_panic(AO_PANIC_REBOOT);
 263 }
 264
 265 static void
 266 version(void)
 267 {
 268         printf("manufacturer     %s\n"
 269                "product          %s\n"
 270                "serial-number    %u\n"
 271 #if HAS_FLIGHT
 272                "current-flight   %u\n"
 273 #endif
 274 #if HAS_LOG
 275                "log-format       %u\n"
 276 #endif
 277                , ao_manufacturer
 278                , ao_product
 279                , ao_serial_number
 280 #if HAS_FLIGHT
 281                , ao_flight_number
 282 #endif
 283 #if HAS_LOG
 284                , ao_log_format
 285 #endif
 286                 );
 287 #if HAS_MS5607
 288         ao_ms5607_info();
 289 #endif
 290         printf("software-version %s\n", ao_version);
 291 }
 292
 293 #ifndef NUM_CMDS
 294 #define NUM_CMDS        11
 295 #endif
 296
 297 static __code struct ao_cmds    *__xdata (ao_cmds[NUM_CMDS]);
 298 static __pdata uint8_t          ao_ncmds;
 299
 300 static void
 301 help(void)
 302 {
 303         __pdata uint8_t cmds;
 304         __pdata uint8_t cmd;
 305         __code struct ao_cmds * __pdata cs;
 306         __code const char *h;
 307         uint8_t e;
 308
 309         for (cmds = 0; cmds < ao_ncmds; cmds++) {
 310                 cs = ao_cmds[cmds];
 311                 for (cmd = 0; cs[cmd].func; cmd++) {
 312                         h = cs[cmd].help;
 313                         ao_put_string(h);
 314                         e = strlen(h);
 315                         h += e + 1;
 316                         e = 45 - e;
 317                         while (e--)
 318                                 putchar(' ');
 319                         ao_put_string(h);
 320                         putchar('\n');
 321                 }
 322         }
 323 }
 324
 325 static void
 326 report(void)
 327 {
 328         switch(ao_cmd_status) {
 329         case ao_cmd_lex_error:
 330         case ao_cmd_syntax_error:
 331                 puts("Syntax error");
 332                 ao_cmd_status = 0;
 333         default:
 334                 break;
 335         }
 336 }
 337
 338 void
 339 ao_cmd_register(__code struct ao_cmds *cmds)
 340 {
 341         if (ao_ncmds >= NUM_CMDS)
 342                 ao_panic(AO_PANIC_CMD);
 343         ao_cmds[ao_ncmds++] = cmds;
 344 }
 345
 346 void
 347 ao_cmd(void)
 348 {
 349         __pdata char    c;
 350         uint8_t cmd, cmds;
 351         __code struct ao_cmds * __xdata cs;
 352         void (*__xdata func)(void);
 353
 354         for (;;) {
 355                 readline();
 356                 ao_cmd_lex();
 357                 ao_cmd_white();
 358                 c = ao_cmd_lex_c;
 359                 ao_cmd_lex();
 360                 if (c == '\r' || c == '\n')
 361                         continue;
 362                 func = (void (*)(void)) NULL;
 363                 for (cmds = 0; cmds < ao_ncmds; cmds++) {
 364                         cs = ao_cmds[cmds];
 365                         for (cmd = 0; cs[cmd].func; cmd++)
 366                                 if (cs[cmd].help[0] == c) {
 367                                         func = cs[cmd].func;
 368                                         break;
 369                                 }
 370                         if (func)
 371                                 break;
 372                 }
 373                 if (func)
 374                         (*func)();
 375                 else
 376                         ao_cmd_status = ao_cmd_syntax_error;
 377                 report();
 378         }
 379 }
 380
 381 __xdata struct ao_task ao_cmd_task;
 382
 383 __code struct ao_cmds   ao_base_cmds[] = {
 384         { help,         "?\0Help" },
 385         { ao_task_info, "T\0Tasks" },
 386         { echo,         "E <0 off, 1 on>\0Echo" },
 387         { ao_reboot,    "r eboot\0Reboot" },
 388         { version,      "v\0Version" },
 389         { 0,    NULL },
 390 };
 391
 392 void
 393 ao_cmd_init(void)
 394 {
 395         ao_cmd_register(&ao_base_cmds[0]);
 396         ao_add_task(&ao_cmd_task, ao_cmd, "cmd");
 397 }