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