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