git.gag.com Git - fw/altos/blob - 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 #define LEX_ERROR       1
  21 #define SYNTAX_ERROR    2
  22 #define SUCCESS         0
  23
  24 static __xdata uint16_t lex_i;
  25 static __xdata uint8_t  lex_c;
  26 static __xdata uint8_t  lex_status;
  27 static __xdata uint8_t  lex_echo;
  28
  29 #define CMD_LEN 32
  30
  31 static __xdata uint8_t  cmd_line[CMD_LEN];
  32 static __xdata uint8_t  cmd_len;
  33 static __xdata uint8_t  cmd_i;
  34
  35 void
  36 putchar(char c)
  37 {
  38         if (c == '\n')
  39                 ao_usb_putchar('\r');
  40         ao_usb_putchar((uint8_t) c);
  41 }
  42
  43 void
  44 flush(void)
  45 {
  46         ao_usb_flush();
  47 }
  48
  49 char
  50 getchar(void)
  51 {
  52         return (char) ao_usb_getchar();
  53 }
  54
  55 static void
  56 put_string(char *s)
  57 {
  58         __xdata uint8_t c;
  59         while (c = *s++)
  60                 putchar(c);
  61 }
  62
  63 static void
  64 readline(void)
  65 {
  66         __xdata uint8_t c;
  67         if (lex_echo)
  68                 put_string("> ");
  69         cmd_len = 0;
  70         for (;;) {
  71                 flush();
  72                 c = getchar();
  73                 /* backspace/delete */
  74                 if (c == '\010' || c == '\177') {
  75                         if (cmd_len != 0) {
  76                                 if (lex_echo)
  77                                         put_string("\010 \010");
  78                                 --cmd_len;
  79                         }
  80                         continue;
  81                 }
  82
  83                 /* ^U */
  84                 if (c == '\025') {
  85                         while (cmd_len != 0) {
  86                                 if (lex_echo)
  87                                         put_string("\010 \010");
  88                                 --cmd_len;
  89                         }
  90                         continue;
  91                 }
  92
  93                 /* map CR to NL */
  94                 if (c == '\r')
  95                         c = '\n';
  96
  97                 if (c == '\n') {
  98                         if (lex_echo)
  99                                 put_string ("\n");
 100                         break;
 101                 }
 102
 103                 if (cmd_len >= CMD_LEN - 2) {
 104                         if (lex_echo)
 105                                 putchar('\007');
 106                         continue;
 107                 }
 108                 cmd_line[cmd_len++] = c;
 109                 if (lex_echo)
 110                         putchar(c);
 111         }
 112         cmd_line[cmd_len++] = '\n';
 113         cmd_line[cmd_len++] = '\0';
 114         cmd_i = 0;
 115 }
 116
 117 static void
 118 lex(void)
 119 {
 120         lex_c = '\n';
 121         if (cmd_i < cmd_len)
 122                 lex_c = cmd_line[cmd_i++];
 123 }
 124
 125 static void
 126 putnibble(uint8_t v)
 127 {
 128         if (v < 10)
 129                 putchar(v + '0');
 130         else
 131                 putchar(v + ('a' - 10));
 132 }
 133
 134 void
 135 put16(uint16_t v)
 136 {
 137         int8_t i;
 138         for (i = 3; i >= 0; i--)
 139                 putnibble((v >> (i << 2)) & 0xf);
 140 }
 141
 142 void
 143 put8(uint8_t v)
 144 {
 145         putnibble((v >> 4) & 0xf);
 146         putnibble(v & 0xf);
 147 }
 148
 149 #define NUM_LEN 7
 150
 151 void
 152 puti(int i)
 153 {
 154         static uint8_t __xdata  num_buffer[NUM_LEN];
 155         uint8_t __xdata * __xdata num_ptr = num_buffer + NUM_LEN;
 156         uint8_t __xdata neg = 0;
 157
 158         *--num_ptr = '\0';
 159         if (i < 0) {
 160                 i = -i;
 161                 neg = 1;
 162         }
 163         do {
 164                 *--num_ptr = '0' + i % 10;
 165                 i /= 10;
 166         } while (i);
 167         if (neg)
 168                 *--num_ptr = '-';
 169         while (num_ptr != num_buffer)
 170                 *--num_ptr = ' ';
 171         put_string(num_buffer);
 172 }
 173
 174
 175 static void
 176 white(void)
 177 {
 178         while (lex_c == ' ' || lex_c == '\t')
 179                 lex();
 180 }
 181
 182 static void
 183 hex(void)
 184 {
 185         __xdata uint8_t r = LEX_ERROR;
 186
 187         lex_i = 0;
 188         white();
 189         for(;;) {
 190                 if ('0' <= lex_c && lex_c <= '9')
 191                         lex_i = (lex_i << 4) | (lex_c - '0');
 192                 else if ('a' <= lex_c && lex_c <= 'f')
 193                         lex_i = (lex_i << 4) | (lex_c - 'a' + 10);
 194                 else if ('A' <= lex_c && lex_c <= 'F')
 195                         lex_i = (lex_i << 4) | (lex_c - 'A' + 10);
 196                 else
 197                         break;
 198                 r = SUCCESS;
 199                 lex();
 200         }
 201         if (r != SUCCESS)
 202                 lex_status = r;
 203 }
 204
 205 #if 0
 206 static void
 207 decimal(void)
 208 {
 209         __xdata uint8_t r = LEX_ERROR;
 210
 211         lex_i = 0;
 212         white();
 213         for(;;) {
 214                 if ('0' <= lex_c && lex_c <= '9')
 215                         lex_i = (lex_i * 10 ) | (lex_c - '0');
 216                 else
 217                         break;
 218                 r = SUCCESS;
 219                 lex();
 220         }
 221         if (r != SUCCESS)
 222                 lex_status = r;
 223 }
 224 #endif
 225
 226 static void
 227 eol(void)
 228 {
 229         while (lex_c != '\n')
 230                 lex();
 231 }
 232
 233 static void
 234 adc_dump(void)
 235 {
 236         __xdata struct ao_adc   packet;
 237         ao_adc_get(&packet);
 238         put_string("tick: ");
 239         puti(packet.tick);
 240         put_string(" accel: ");
 241         puti(packet.accel >> 4);
 242         put_string(" pres: ");
 243         puti(packet.pres >> 4);
 244         put_string(" temp: ");
 245         puti(packet.temp >> 4);
 246         put_string(" batt: ");
 247         puti(packet.v_batt >> 4);
 248         put_string(" drogue: ");
 249         puti(packet.sense_d >> 4);
 250         put_string(" main: ");
 251         puti(packet.sense_m >> 4);
 252         put_string("\n");
 253 }
 254
 255 static void
 256 gps_dump(void) __reentrant
 257 {
 258         ao_mutex_get(&ao_gps_mutex);
 259         if (ao_gps_data.flags & AO_GPS_VALID) {
 260                 printf("GPS %2d:%02d:%02d %2d°%2d.%04d'%c %2d°%2d.%04d'%c %5dm %2d sat\n",
 261                        ao_gps_data.hour,
 262                        ao_gps_data.minute,
 263                        ao_gps_data.second,
 264                        ao_gps_data.latitude.degrees,
 265                        ao_gps_data.latitude.minutes,
 266                        ao_gps_data.latitude.minutes_fraction,
 267                        (ao_gps_data.flags & AO_GPS_LATITUDE_MASK) == AO_GPS_LATITUDE_NORTH ?
 268                        'N' : 'S',
 269                        ao_gps_data.longitude.degrees,
 270                        ao_gps_data.longitude.minutes,
 271                        ao_gps_data.longitude.minutes_fraction,
 272                        (ao_gps_data.flags & AO_GPS_LONGITUDE_MASK) == AO_GPS_LONGITUDE_WEST ?
 273                        'W' : 'E',
 274                        ao_gps_data.altitude,
 275                        (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
 276         } else {
 277                 printf("GPS %2d sat\n",
 278                        (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);;
 279         }
 280         ao_mutex_put(&ao_gps_mutex);
 281 }
 282
 283 static void
 284 dump(void)
 285 {
 286         __xdata uint16_t c;
 287         __xdata uint8_t * __xdata start, * __xdata end;
 288
 289         hex();
 290         start = (uint8_t __xdata *) lex_i;
 291         hex();
 292         end = (uint8_t __xdata *) lex_i;
 293         if (lex_status != SUCCESS)
 294                 return;
 295         c = 0;
 296         while (start <= end) {
 297                 if ((c & 7) == 0) {
 298                         if (c)
 299                                 put_string("\n");
 300                         put16((uint16_t) start);
 301                 }
 302                 putchar(' ');
 303                 put8(*start);
 304                 ++c;
 305                 start++;
 306         }
 307         put_string("\n");
 308 }
 309
 310 static void
 311 ee_dump(void)
 312 {
 313         __xdata uint8_t b;
 314         __xdata uint16_t block;
 315         __xdata uint8_t i;
 316
 317         hex();
 318         block = lex_i;
 319         if (lex_status != SUCCESS)
 320                 return;
 321         i = 0;
 322         do {
 323                 if ((i & 7) == 0) {
 324                         if (i)
 325                                 put_string("\n");
 326                         put16((uint16_t) i);
 327                 }
 328                 putchar(' ');
 329                 ao_ee_read(((uint32_t) block << 8) | i, &b, 1);
 330                 put8(b);
 331                 ++i;
 332         } while (i != 0);
 333         put_string("\n");
 334 }
 335
 336 static void
 337 ee_store(void)
 338 {
 339         __xdata uint16_t block;
 340         __xdata uint8_t i;
 341         __xdata uint16_t len;
 342         __xdata uint8_t b;
 343         __xdata uint32_t addr;
 344
 345         hex();
 346         block = lex_i;
 347         hex();
 348         i = lex_i;
 349         addr = ((uint32_t) block << 8) | i;
 350         hex();
 351         len = lex_i;
 352         if (lex_status != SUCCESS)
 353                 return;
 354         while (len--) {
 355                 hex();
 356                 if (lex_status != SUCCESS)
 357                         return;
 358                 b = lex_i;
 359                 ao_ee_write(addr, &b, 1);
 360                 addr++;
 361         }
 362         ao_ee_flush();
 363 }
 364
 365 static void
 366 echo(void)
 367 {
 368         hex();
 369         lex_echo = lex_i != 0;
 370 }
 371
 372 static void
 373 debug_enable(void)
 374 {
 375         ao_dbg_debug_mode();
 376 }
 377
 378 static void
 379 debug_reset(void)
 380 {
 381         ao_dbg_reset();
 382 }
 383
 384 static void
 385 debug_put(void)
 386 {
 387         for (;;) {
 388                 white ();
 389                 if (lex_c == '\n')
 390                         break;
 391                 hex();
 392                 if (lex_status != SUCCESS)
 393                         break;
 394                 ao_dbg_send_byte(lex_i);
 395         }
 396 }
 397
 398 static void
 399 debug_get(void)
 400 {
 401         __xdata uint16_t count;
 402         __xdata uint16_t i;
 403         __xdata uint8_t byte;
 404         hex();
 405         if (lex_status != SUCCESS)
 406                 return;
 407         count = lex_i;
 408         if (count > 256) {
 409                 lex_status = SYNTAX_ERROR;
 410                 return;
 411         }
 412         for (i = 0; i < count; i++) {
 413                 if (i && (i & 7) == 0)
 414                         put_string("\n");
 415                 byte = ao_dbg_recv_byte();
 416                 put8(byte);
 417                 putchar(' ');
 418         }
 419         put_string("\n");
 420 }
 421
 422 static uint8_t
 423 getnibble(void)
 424 {
 425         __xdata uint8_t c;
 426
 427         c = getchar();
 428         if ('0' <= c && c <= '9')
 429                 return c - '0';
 430         if ('a' <= c && c <= 'f')
 431                 return c - ('a' - 10);
 432         if ('A' <= c && c <= 'F')
 433                 return c - ('A' - 10);
 434         lex_status = LEX_ERROR;
 435         return 0;
 436 }
 437
 438 static void
 439 debug_input(void)
 440 {
 441         __xdata uint16_t count;
 442         __xdata uint16_t addr;
 443         __xdata uint8_t b;
 444         __xdata uint8_t i;
 445
 446         hex();
 447         count = lex_i;
 448         hex();
 449         addr = lex_i;
 450         if (lex_status != SUCCESS)
 451                 return;
 452         ao_dbg_start_transfer(addr);
 453         i = 0;
 454         while (count--) {
 455                 if (!(i++ & 7))
 456                         put_string("\n");
 457                 b = ao_dbg_read_byte();
 458                 put8(b);
 459         }
 460         ao_dbg_end_transfer();
 461         put_string("\n");
 462 }
 463
 464 static void
 465 debug_output(void)
 466 {
 467         __xdata uint16_t count;
 468         __xdata uint16_t addr;
 469         __xdata uint8_t b;
 470
 471         hex();
 472         count = lex_i;
 473         hex();
 474         addr = lex_i;
 475         if (lex_status != SUCCESS)
 476                 return;
 477         ao_dbg_start_transfer(addr);
 478         while (count--) {
 479                 b = getnibble() << 4;
 480                 b |= getnibble();
 481                 if (lex_status != SUCCESS)
 482                         return;
 483                 ao_dbg_write_byte(b);
 484         }
 485         ao_dbg_end_transfer();
 486 }
 487
 488 static void
 489 dump_log(void)
 490 {
 491         __xdata uint8_t more;
 492
 493         for (more = ao_log_dump_first(); more; more = ao_log_dump_next()) {
 494                 putchar(ao_log_dump.type);
 495                 putchar(' ');
 496                 put16(ao_log_dump.tick);
 497                 putchar(' ');
 498                 put16(ao_log_dump.u.anon.d0);
 499                 putchar(' ');
 500                 put16(ao_log_dump.u.anon.d1);
 501                 putchar('\n');
 502         }
 503 }
 504
 505 static void
 506 send_serial(void)
 507 {
 508         white();
 509         while (lex_c != '\n') {
 510                 ao_serial_putchar(lex_c);
 511                 lex();
 512         }
 513 }
 514
 515 static const uint8_t help_txt[] =
 516         "All numbers are in hex\n"
 517         "?                                  Print this message\n"
 518         "a                                  Display current ADC values\n"
 519         "g                                  Display current GPS values\n"
 520         "d <start> <end>                    Dump memory\n"
 521         "e <block>                          Dump a block of EEPROM data\n"
 522         "w <block> <start> <len> <data> ... Write data to EEPROM\n"
 523         "l                                  Dump last flight log\n"
 524         "E <0 off, 1 on>                    Set command echo mode\n"
 525         "S<data>                            Send data to serial line\n"
 526         "T                                  Show task states\n"
 527         "\n"
 528         "Target debug commands:\n"
 529         "D                                  Enable debug mode\n"
 530         "R                                  Reset target\n"
 531         "P <byte> ...                       Put data to debug port\n"
 532         "G <count>                          Get data from debug port\n"
 533         "O <count> <addr>                   Output <count> bytes to target at <addr>\n"
 534         "I <count> <addr>                   Input <count> bytes to target at <addr>\n"
 535 ;
 536
 537 static void
 538 help(void)
 539 {
 540         put_string(help_txt);
 541 }
 542
 543 static void
 544 report(void)
 545 {
 546         switch(lex_status) {
 547         case LEX_ERROR:
 548         case SYNTAX_ERROR:
 549                 put_string("Syntax error\n");
 550                 lex_status = 0;
 551                 break;
 552         }
 553 }
 554
 555 void
 556 ao_cmd(void *parameters)
 557 {
 558         __xdata uint8_t c;
 559         (void) parameters;
 560
 561         lex_echo = 1;
 562         for (;;) {
 563                 readline();
 564                 lex();
 565                 white();
 566                 c = lex_c;
 567                 lex();
 568                 switch (c) {
 569                 case '?':
 570                         help();
 571                         break;
 572                 case 'd':
 573                         dump();
 574                         break;
 575                 case 'a':
 576                         adc_dump();
 577                         break;
 578                 case 'g':
 579                         gps_dump();
 580                         break;
 581                 case 'e':
 582                         ee_dump();
 583                         break;
 584                 case 'w':
 585                         ee_store();
 586                         break;
 587                 case 'l':
 588                         dump_log();
 589                         break;
 590                 case 'T':
 591                         ao_task_info();
 592                         break;
 593                 case 'S':
 594                         send_serial();
 595                         break;
 596                 case 'E':
 597                         echo();
 598                         break;
 599                 case 'D':
 600                         debug_enable();
 601                         break;
 602                 case 'R':
 603                         debug_reset();
 604                         break;
 605                 case 'P':
 606                         debug_put();
 607                         break;
 608                 case 'G':
 609                         debug_get();
 610                         break;
 611                 case 'I':
 612                         debug_input();
 613                         break;
 614                 case 'O':
 615                         debug_output();
 616                         break;
 617                 case '\r':
 618                 case '\n':
 619                         break;
 620                 default:
 621                         lex_status = SYNTAX_ERROR;
 622                         break;
 623                 }
 624                 report();
 625         }
 626
 627 }
 628
 629 __xdata struct ao_task ao_cmd_task;
 630
 631 void
 632 ao_cmd_init(void)
 633 {
 634         ao_add_task(&ao_cmd_task, ao_cmd, "cmd");
 635 }