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 __pdata uint16_t ao_cmd_lex_i;
  23 __pdata uint32_t ao_cmd_lex_u32;
  24 __pdata char    ao_cmd_lex_c;
  25 __pdata enum ao_cmd_status ao_cmd_status;
  26
  27 #if AO_PYRO_NUM
  28 #define CMD_LEN 128
  29 #else
  30 #define CMD_LEN 48
  31 #endif
  32
  33 static __xdata char     cmd_line[CMD_LEN];
  34 static __pdata uint8_t  cmd_len;
  35 static __pdata uint8_t  cmd_i;
  36
  37 void
  38 ao_put_string(__code 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 static void
  52 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 >= 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 void
 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 }
 109
 110 static void
 111 putnibble(uint8_t v)
 112 {
 113         if (v < 10)
 114                 putchar(v + '0');
 115         else
 116                 putchar(v + ('a' - 10));
 117 }
 118
 119 uint8_t
 120 ao_getnibble(void)
 121 {
 122         char    c;
 123
 124         c = getchar();
 125         if ('0' <= c && c <= '9')
 126                 return c - '0';
 127         if ('a' <= c && c <= 'f')
 128                 return c - ('a' - 10);
 129         if ('A' <= c && c <= 'F')
 130                 return c - ('A' - 10);
 131         ao_cmd_status = ao_cmd_lex_error;
 132         return 0;
 133 }
 134
 135 void
 136 ao_cmd_put16(uint16_t v)
 137 {
 138         ao_cmd_put8(v >> 8);
 139         ao_cmd_put8(v);
 140 }
 141
 142 void
 143 ao_cmd_put8(uint8_t v)
 144 {
 145         putnibble((v >> 4) & 0xf);
 146         putnibble(v & 0xf);
 147 }
 148
 149 uint8_t
 150 ao_cmd_is_white(void)
 151 {
 152         return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t';
 153 }
 154
 155 void
 156 ao_cmd_white(void)
 157 {
 158         while (ao_cmd_is_white())
 159                 ao_cmd_lex();
 160 }
 161
 162 int8_t
 163 ao_cmd_hexchar(char c)
 164 {
 165         if ('0' <= c && c <= '9')
 166                 return (c - '0');
 167         if ('a' <= c && c <= 'f')
 168                 return (c - 'a' + 10);
 169         if ('A' <= c && c <= 'F')
 170                 return (c - 'A' + 10);
 171         return -1;
 172 }
 173
 174 void
 175 ao_cmd_hexbyte(void)
 176 {
 177         uint8_t i;
 178         int8_t  n;
 179
 180         ao_cmd_lex_i = 0;
 181         ao_cmd_white();
 182         for (i = 0; i < 2; i++) {
 183                 n = ao_cmd_hexchar(ao_cmd_lex_c);
 184                 if (n < 0) {
 185                         ao_cmd_status = ao_cmd_syntax_error;
 186                         break;
 187                 }
 188                 ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
 189                 ao_cmd_lex();
 190         }
 191 }
 192
 193 void
 194 ao_cmd_hex(void)
 195 {
 196         __pdata uint8_t r = ao_cmd_lex_error;
 197         int8_t  n;
 198
 199         ao_cmd_lex_i = 0;
 200         ao_cmd_white();
 201         for(;;) {
 202                 n = ao_cmd_hexchar(ao_cmd_lex_c);
 203                 if (n < 0)
 204                         break;
 205                 ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
 206                 r = ao_cmd_success;
 207                 ao_cmd_lex();
 208         }
 209         if (r != ao_cmd_success)
 210                 ao_cmd_status = r;
 211 }
 212
 213 void
 214 ao_cmd_decimal(void) __reentrant
 215 {
 216         uint8_t r = ao_cmd_lex_error;
 217
 218         ao_cmd_lex_u32 = 0;
 219         ao_cmd_white();
 220         for(;;) {
 221                 if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9')
 222                         ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0');
 223                 else
 224                         break;
 225                 r = ao_cmd_success;
 226                 ao_cmd_lex();
 227         }
 228         if (r != ao_cmd_success)
 229                 ao_cmd_status = r;
 230         ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32;
 231 }
 232
 233 uint8_t
 234 ao_match_word(__code char *word)
 235 {
 236         while (*word) {
 237                 if (ao_cmd_lex_c != *word) {
 238                         ao_cmd_status = ao_cmd_syntax_error;
 239                         return 0;
 240                 }
 241                 word++;
 242                 ao_cmd_lex();
 243         }
 244         return 1;
 245 }
 246
 247 static void
 248 echo(void)
 249 {
 250         ao_cmd_hex();
 251         if (ao_cmd_status == ao_cmd_success)
 252                 ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0;
 253 }
 254
 255 static void
 256 ao_reboot(void)
 257 {
 258         ao_cmd_white();
 259         if (!ao_match_word("eboot"))
 260                 return;
 261         /* Delay waiting for the packet master to be turned off
 262          * so that we don't end up back in idle mode because we
 263          * received a packet after boot.
 264          */
 265         flush();
 266         ao_delay(AO_SEC_TO_TICKS(1));
 267         ao_arch_reboot();
 268         ao_panic(AO_PANIC_REBOOT);
 269 }
 270
 271 #ifndef HAS_VERSION
 272 #define HAS_VERSION 1
 273 #endif
 274
 275 #if HAS_VERSION
 276 static void
 277 version(void)
 278 {
 279         printf("manufacturer     %s\n"
 280                "product          %s\n"
 281                "serial-number    %u\n"
 282 #if HAS_FLIGHT || HAS_TRACKER
 283                "current-flight   %u\n"
 284 #endif
 285 #if HAS_LOG
 286                "log-format       %u\n"
 287 #if !DISABLE_LOG_SPACE
 288                "log-space        %lu\n"
 289 #endif
 290 #endif
 291 #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
 292                "program-space    %u\n"
 293 #endif
 294 #if AO_VALUE_32
 295                "altitude-32      1\n"
 296 #endif
 297                , ao_manufacturer
 298                , ao_product
 299                , ao_serial_number
 300 #if HAS_FLIGHT || HAS_TRACKER
 301                , ao_flight_number
 302 #endif
 303 #if HAS_LOG
 304                , ao_log_format
 305 #if !DISABLE_LOG_SPACE
 306                , (unsigned long) ao_storage_log_max
 307 #endif
 308 #endif
 309 #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
 310                , (uint32_t) AO_BOOT_APPLICATION_BOUND - (uint32_t) AO_BOOT_APPLICATION_BASE
 311 #endif
 312                 );
 313         printf("software-version %s\n", ao_version);
 314 }
 315 #endif
 316
 317 #ifndef NUM_CMDS
 318 #define NUM_CMDS        11
 319 #endif
 320
 321 static __code struct ao_cmds    *__xdata (ao_cmds[NUM_CMDS]);
 322 static __pdata uint8_t          ao_ncmds;
 323
 324 static void
 325 help(void)
 326 {
 327         __pdata uint8_t cmds;
 328         __pdata uint8_t cmd;
 329         __code struct ao_cmds * __pdata cs;
 330         __code const char *h;
 331         uint8_t e;
 332
 333         for (cmds = 0; cmds < ao_ncmds; cmds++) {
 334                 cs = ao_cmds[cmds];
 335                 for (cmd = 0; cs[cmd].func; cmd++) {
 336                         h = cs[cmd].help;
 337                         ao_put_string(h);
 338                         e = strlen(h);
 339                         h += e + 1;
 340                         e = 45 - e;
 341                         while (e--)
 342                                 putchar(' ');
 343                         ao_put_string(h);
 344                         putchar('\n');
 345                 }
 346         }
 347 }
 348
 349 static void
 350 report(void)
 351 {
 352         switch(ao_cmd_status) {
 353         case ao_cmd_lex_error:
 354         case ao_cmd_syntax_error:
 355                 puts("Syntax error");
 356                 ao_cmd_status = 0;
 357         default:
 358                 break;
 359         }
 360 }
 361
 362 void
 363 ao_cmd_register(__code struct ao_cmds *cmds)
 364 {
 365         if (ao_ncmds >= NUM_CMDS)
 366                 ao_panic(AO_PANIC_CMD);
 367         ao_cmds[ao_ncmds++] = cmds;
 368 }
 369
 370 void
 371 ao_cmd(void)
 372 {
 373         __pdata char    c;
 374         uint8_t cmd, cmds;
 375         __code struct ao_cmds * __xdata cs;
 376         void (*__xdata func)(void);
 377
 378         for (;;) {
 379                 readline();
 380                 ao_cmd_lex();
 381                 ao_cmd_white();
 382                 c = ao_cmd_lex_c;
 383                 ao_cmd_lex();
 384                 if (c == '\r' || c == '\n')
 385                         continue;
 386                 func = (void (*)(void)) NULL;
 387                 for (cmds = 0; cmds < ao_ncmds; cmds++) {
 388                         cs = ao_cmds[cmds];
 389                         for (cmd = 0; cs[cmd].func; cmd++)
 390                                 if (cs[cmd].help[0] == c) {
 391                                         func = cs[cmd].func;
 392                                         break;
 393                                 }
 394                         if (func)
 395                                 break;
 396                 }
 397                 if (func)
 398                         (*func)();
 399                 else
 400                         ao_cmd_status = ao_cmd_syntax_error;
 401                 report();
 402         }
 403 }
 404
 405 #if HAS_BOOT_LOADER
 406
 407 #include <ao_boot.h>
 408
 409 static void
 410 ao_loader(void)
 411 {
 412         flush();
 413         ao_boot_loader();
 414 }
 415 #endif
 416
 417 __xdata struct ao_task ao_cmd_task;
 418
 419 __code struct ao_cmds   ao_base_cmds[] = {
 420         { help,         "?\0Help" },
 421 #if HAS_TASK_INFO
 422         { ao_task_info, "T\0Tasks" },
 423 #endif
 424         { echo,         "E <0 off, 1 on>\0Echo" },
 425         { ao_reboot,    "r eboot\0Reboot" },
 426 #if HAS_VERSION
 427         { version,      "v\0Version" },
 428 #endif
 429 #if HAS_BOOT_LOADER
 430         { ao_loader,    "X\0Switch to boot loader" },
 431 #endif
 432         { 0,    NULL },
 433 };
 434
 435 void
 436 ao_cmd_init(void)
 437 {
 438         ao_cmd_register(&ao_base_cmds[0]);
 439         ao_add_task(&ao_cmd_task, ao_cmd, "cmd");
 440 }