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