1 /* 2 * styx.c 3 * 4 * A Styx fileserver for a Lego RCX 5 * 6 * Nigel Roles 7 * Vita Nuova 8 * 9 * This is a heavily modified version of test5.c 10 * 11 * I couldn't have done this without Kekoa... 12 * 13 * 14 * The contents of this file are subject to the Mozilla Public License 15 * Version 1.0 (the "License"); you may not use this file except in 16 * compliance with the License. You may obtain a copy of the License at 17 * http://www.mozilla.org/MPL/ 18 * 19 * Software distributed under the License is distributed on an "AS IS" 20 * basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the 21 * License for the specific language governing rights and limitations 22 * under the License. 23 * 24 * The Original Code is Librcx sample program code, released February 9, 25 * 1999. 26 * 27 * The Initial Developer of the Original Code is Kekoa Proudfoot. 28 * Portions created by Kekoa Proudfoot are Copyright (C) 1999 29 * Kekoa Proudfoot. All Rights Reserved. 30 * 31 * Contributor(s): Kekoa Proudfoot <kekoa@graphics.stanford.edu> 32 */ 33 34 //#include "stdlib.h" 35 #include "rom.h" 36 37 #include "lib9.h" 38 #include "styx.h" 39 40 #include "llp.h" 41 42 #define ASSERT(cond) if (!(cond)) fatal(__LINE__) 43 #define FATAL fatal(__LINE__) 44 #define PROGRESS progress(__LINE__) 45 46 #if 0 47 #define ABP 48 #endif 49 50 uchar *send_fid_reply_payload(void); 51 void send_fid_reply(uchar type, ushort tag, ushort fid, uchar *msg, short len); 52 void send_error_reply(unsigned short tag, char *msg); 53 54 static unsigned short msgcount; 55 static unsigned char compressed_incoming[150]; 56 static unsigned char incoming[1024]; 57 static unsigned char compressed_reply[150]; 58 short compressed_reply_len; 59 static unsigned char reply[1024]; 60 unsigned short reply_len; 61 unsigned short transmitted_reply_len; 62 unsigned char alternating_bit; 63 static uchar dir[116]; 64 uchar prepared; 65 uchar reader_count; 66 uchar dispatch[6]; 67 68 /* ROM pseudofunctions */ 69 70 static inline void 71 set_data_pointer (void *ptr) 72 { 73 play_sound_or_set_data_pointer(0x1771, (short)ptr, 0); 74 } 75 76 static inline char 77 check_valid (void) 78 { 79 char valid; 80 check_for_data(&valid, NULL); 81 return valid; 82 } 83 84 static inline int 85 receive_message (void *ptr, int len) 86 { 87 char bytes = 0; 88 receive_data(ptr, len, &bytes); 89 /* Bytes includes checksum, since we don't want that, return bytes-1 */ 90 return bytes - 1; 91 } 92 93 static inline void 94 send_message (void *ptr, int len) 95 { 96 if (len) 97 while (send_data(0x1776, 0, ptr, len)); 98 } 99 100 int 101 poll_power(void) 102 { 103 static short debounce = 0; 104 static short state = -1; 105 short status; 106 get_power_status(0x4000, &status); 107 if (state != status) 108 debounce = 0; 109 else if (debounce < 10) 110 debounce++; 111 state = status; 112 return debounce >= 10 ? state : -1; 113 } 114 115 static void 116 progress(short line) 117 { 118 set_lcd_number(LCD_UNSIGNED, line, LCD_DECIMAL_0); 119 refresh_display(); 120 } 121 122 static void 123 fatal(short line) 124 { 125 set_lcd_segment(LCD_STANDING); 126 progress(line); 127 while (poll_power() != 0) 128 ; 129 } 130 131 typedef struct Reader { 132 ushort tag; 133 ushort fid; 134 ushort offset; 135 ushort count; 136 struct Reader *next; 137 } Reader; 138 139 typedef struct DirectoryEntry { 140 char *name; 141 uchar qid; 142 const struct DirectoryEntry *sub; 143 short (*read)(const struct DirectoryEntry *dp, ushort tag, ushort fid, ushort offset, ushort count); 144 short (*write)(const struct DirectoryEntry *dp, ushort offset, ushort count, uchar *buf); 145 } DirectoryEntry; 146 147 #define QID_ROOT 0 148 #define QID_MOTOR 1 149 #define QID_MOTOR_0 2 150 #define QID_MOTOR_1 3 151 #define QID_MOTOR_2 4 152 #define QID_MOTOR_012 5 153 #define QID_SENSOR 6 154 #define QID_SENSOR_0 7 155 #define QID_SENSOR_1 8 156 #define QID_SENSOR_2 9 157 158 typedef struct Sensor { 159 sensor_t sensor; 160 uchar active; 161 uchar greater; 162 ushort thresh; 163 Reader *reader; 164 } Sensor; 165 166 Sensor sensor[3]; 167 168 short 169 atoin(char *s, short lim) 170 { 171 short total = 0; 172 while (*s && lim) { 173 char c = *s++; 174 if (c >= '0' && c <= '9') 175 total = total * 10 + c - '0'; 176 else 177 break; 178 lim--; 179 } 180 return total; 181 } 182 183 short 184 itoa(char *buf, short value) 185 { 186 char *bp = buf; 187 short divisor; 188 if (value < 0) { 189 *bp++ = '-'; 190 value = -value; 191 } 192 if (value == 0) 193 *bp++ = '0'; 194 else { 195 divisor = 10000; 196 while (divisor > value) 197 divisor /= 10; 198 while (divisor) { 199 *bp++ = '0' + value / divisor; 200 value %= divisor; 201 divisor /= 10; 202 } 203 } 204 return bp - buf; 205 } 206 207 Reader * 208 readercreate(ushort tag, ushort fid, ushort offset, ushort count) 209 { 210 Reader *rp = malloc(sizeof(Reader)); 211 rp->tag = tag; 212 rp->fid = fid; 213 rp->offset = offset; 214 rp->count = count; 215 rp->next = 0; 216 reader_count++; 217 return rp; 218 } 219 220 void 221 readerfree(Reader *rp) 222 { 223 free(rp); 224 reader_count--; 225 } 226 227 int 228 senderrorreset(Reader *rp, void *magic) 229 { 230 send_error_reply(rp->tag, "reset"); 231 return 1; 232 } 233 234 void 235 readerlistfindanddestroy(Reader **rpp, int (*action)(Reader *rp, void *magic), void *magic) 236 { 237 while (*rpp) { 238 Reader *rp = *rpp; 239 if ((*action)(rp, magic)) { 240 *rpp = rp->next; 241 readerfree(rp); 242 } 243 else 244 rpp = &(rp->next); 245 } 246 } 247 248 void 249 allreaderlistfindanddestroy(int (*action)(Reader *rp, void *magic), void *magic) 250 { 251 short i; 252 for (i = 0; i < 3; i++) 253 readerlistfindanddestroy(&sensor[i].reader, action, magic); 254 } 255 256 short 257 sensorwrite(const DirectoryEntry *dp, ushort offset, ushort count, uchar *data) 258 { 259 short i; 260 Sensor *sp; 261 uchar greater; 262 short type, mode; 263 ushort k; 264 265 if (offset != 0) 266 return -1; 267 i = dp->qid - QID_SENSOR_0; 268 sp = &sensor[i]; 269 k = count; 270 if (k == 0) 271 return -1; 272 switch (data[0]) { 273 case 'b': 274 type = SENSOR_TYPE_TOUCH; 275 mode = SENSOR_MODE_PULSE; 276 break; 277 case 'l': 278 type = SENSOR_TYPE_TOUCH; 279 mode = SENSOR_MODE_RAW; 280 break; 281 default: 282 return -1; 283 } 284 data++; k--; 285 if (k == 0) 286 return -1; 287 if (*data == '>') { 288 greater = 1; 289 data++; 290 k--; 291 } 292 else if (*data == '<') { 293 greater = 0; 294 data++; 295 k--; 296 } 297 else 298 greater = 1; 299 if (k == 0) 300 return -1; 301 readerlistfindanddestroy(&sp->reader, senderrorreset, 0); 302 set_sensor_passive(SENSOR_0 + i); 303 sp->sensor.type = type; 304 sp->sensor.mode = mode; 305 sp->thresh = atoin(data, k); 306 sp->sensor.raw = 0; 307 sp->sensor.value = 0; 308 sp->sensor.boolean = 0; 309 sp->active = 1; 310 sp->greater = greater; 311 set_sensor_active(SENSOR_0 + i); 312 return count; 313 } 314 315 void 316 send_read_reply(ushort tag, ushort fid, ushort offset, ushort len, uchar *answer, short answerlen) 317 { 318 uchar *out = send_fid_reply_payload(); 319 ushort actual; 320 if (offset < answerlen) { 321 actual = answerlen - offset; 322 if (actual > len) 323 actual = len; 324 memcpy(out + 3, answer + offset, actual); 325 } 326 else 327 actual = 0; 328 out[0] = actual; 329 out[1] = actual >> 8; 330 out[2] = 0; 331 send_fid_reply(Rread, tag, fid, 0, actual + 3); 332 } 333 334 void 335 send_sensor_read_reply(ushort tag, ushort fid, ushort offset, ushort count, short value) 336 { 337 short answerlen; 338 char answer[8]; 339 /* reply is countlow counthigh pad data[count] */ 340 answerlen = itoa(answer, value); 341 send_read_reply(tag, fid, offset, count, answer, answerlen); 342 } 343 344 int 345 sensortriggered(Sensor *sp) 346 { 347 if (sp->greater) 348 return sp->sensor.value >= sp->thresh; 349 else 350 return sp->sensor.value < sp->thresh; 351 } 352 353 short 354 sensorread(const struct DirectoryEntry *dp, ushort tag, ushort fid, ushort offset, ushort count) 355 { 356 short i; 357 Sensor *sp; 358 i = dp->qid - QID_SENSOR_0; 359 sp = sensor + i; 360 if (!sp->active) 361 return -1; 362 if (sensortriggered(sp)) 363 send_sensor_read_reply(tag, fid, offset, count, sp->sensor.value); 364 else { 365 /* add to queue */ 366 Reader *rp = readercreate(tag, fid, offset, count); 367 rp->next = sp->reader; 368 sp->reader = rp; 369 } 370 return 0; 371 } 372 373 void 374 sensorpoll(void) 375 { 376 short i; 377 Sensor *sp; 378 379 if ((dispatch[0] & 0x80) == 0) { 380 return; 381 } 382 dispatch[0] &= 0x7f; 383 /* do the following every 3 ms with a following wind */ 384 for (i = 0; i < 3; i++) { 385 sp = sensor + i; 386 if (sp->active) { 387 /* 388 * read sensor 4 times to reduce debounce on each 389 * edge to effectively 25 counts, or 75ms 390 * allowing about 8 pulses a second 391 */ 392 read_sensor(SENSOR_0 + i, &sp->sensor); 393 read_sensor(SENSOR_0 + i, &sp->sensor); 394 read_sensor(SENSOR_0 + i, &sp->sensor); 395 read_sensor(SENSOR_0 + i, &sp->sensor); 396 if (sensortriggered(sp)) { 397 /* complete any outstanding reads */ 398 while (sp->reader) { 399 Reader *rp = sp->reader; 400 sp->reader = rp->next; 401 send_sensor_read_reply(rp->tag, rp->fid, rp->offset, rp->count, sp->sensor.value); 402 readerfree(rp); 403 } 404 } 405 } 406 } 407 } 408 409 short 410 motorparse(uchar *flag, short *mode, short *power, uchar *data) 411 { 412 switch (data[0]) { 413 case 'f': *mode = MOTOR_FWD; break; 414 case 'r': *mode = MOTOR_REV; break; 415 case 's': *mode = MOTOR_STOP; break; 416 case 'F': *mode = MOTOR_FLOAT; break; 417 case '-': return 1; 418 default: 419 return 0; 420 } 421 if (data[1] >= '0' && data[1] <= '7') 422 *power = data[1] - '0'; 423 else 424 return 0; 425 *flag = 1; 426 return 1; 427 } 428 429 short 430 motorwrite(const DirectoryEntry *dp, ushort offset, ushort count, uchar *data) 431 { 432 short mode[3], power[3]; 433 uchar flag[3]; 434 short i; 435 436 if (offset != 0) 437 return -1; 438 flag[0] = flag[1] = flag[2] = 0; 439 if (dp->qid == QID_MOTOR_012) { 440 if (count != 6) 441 return -1; 442 if (!motorparse(flag, mode, power, data) 443 || !motorparse(flag + 1, mode + 1, power + 1, data + 2) 444 || !motorparse(flag + 2, mode + 2, power + 2, data + 4)) 445 return -1; 446 } 447 else { 448 if (count != 2) 449 return -1; 450 i = dp->qid - QID_MOTOR_0; 451 if (!motorparse(flag + i, mode + i, power + i, data)) 452 return -1; 453 } 454 for (i = 0; i < 3; i++) 455 if (flag[i]) 456 control_motor(MOTOR_0 + i, mode[i], power[i]); 457 return count; 458 } 459 460 const uchar qid_root[8] = { QID_ROOT, 0, 0, 0x80 }; 461 462 const DirectoryEntry dir_root[], dir_slash[]; 463 464 const DirectoryEntry dir_motor[] = { 465 { "..", QID_ROOT, dir_root }, 466 { "0", QID_MOTOR_0, 0, 0, motorwrite }, 467 { "1", QID_MOTOR_1, 0, 0, motorwrite }, 468 { "2", QID_MOTOR_2, 0, 0, motorwrite }, 469 { "012", QID_MOTOR_012, 0, 0, motorwrite }, 470 { 0 } 471 }; 472 473 const DirectoryEntry dir_sensor[] = { 474 { "..", QID_ROOT, dir_root }, 475 { "0", QID_SENSOR_0, 0, sensorread, sensorwrite }, 476 { "1", QID_SENSOR_1, 0, sensorread, sensorwrite }, 477 { "2", QID_SENSOR_2, 0, sensorread, sensorwrite }, 478 { 0 } 479 }; 480 481 const DirectoryEntry dir_root[] = { 482 { "..", QID_ROOT, dir_slash }, 483 { "motor", QID_MOTOR, dir_motor }, 484 { "sensor", QID_SENSOR, dir_sensor }, 485 { 0 } 486 }; 487 488 const DirectoryEntry dir_slash[] = { 489 { "/", QID_ROOT, dir_root }, 490 { 0 } 491 }; 492 493 const DirectoryEntry *qid_map[] = { 494 /* QID_ROOT */ &dir_slash[0], 495 /* QID_MOTOR */ &dir_root[1], 496 /* QID_MOTOR_0 */ &dir_motor[1], 497 /* QID_MOTOR_1 */ &dir_motor[2], 498 /* QID_MOTOR_2 */ &dir_motor[3], 499 /* QID_MOTOR_012 */ &dir_motor[4], 500 /* QID_SENSOR */ &dir_root[2], 501 /* QID_SENSOR_0 */ &dir_sensor[1], 502 /* QID_SENSOR_1 */ &dir_sensor[2], 503 /* QID_SENSOR_2 */ &dir_sensor[3], 504 }; 505 506 #define QID_MAP_MAX (sizeof(qid_map) / sizeof(qid_map[0])) 507 508 typedef struct Fid { 509 struct Fid *next; 510 ushort fid; 511 uchar open; 512 uchar qid[8]; 513 } Fid; 514 515 Fid *fids; 516 517 Fid * 518 fidfind(ushort fid) 519 { 520 Fid *fp; 521 for (fp = fids; fp && fp->fid != fid; fp = fp->next) 522 ; 523 return fp; 524 } 525 526 Fid * 527 fidcreate(ushort fid, const uchar qid[8]) 528 { 529 Fid *fp; 530 fp = malloc(sizeof(Fid)); 531 ASSERT(fp); 532 fp->open = 0; 533 fp->fid = fid; 534 fp->next = fids; 535 memcpy(fp->qid, qid, 8); 536 fids = fp; 537 return fp; 538 } 539 540 int 541 matchfp(Reader *rp, void *magic) 542 { 543 if (rp->fid == ((Fid *)magic)->fid) { 544 return 1; 545 } 546 return 0; 547 } 548 549 void 550 fiddelete(Fid *fp) 551 { 552 Fid **fpp; 553 /* clobber any outstanding reads on this fid */ 554 allreaderlistfindanddestroy(matchfp, fp); 555 /* now clobber the fid */ 556 for (fpp = &fids; *fpp; fpp = &(*fpp)->next) 557 if (*fpp == fp) { 558 *fpp = fp->next; 559 free(fp); 560 return; 561 } 562 FATAL; 563 } 564 565 const DirectoryEntry * 566 nthentry(const DirectoryEntry *dp, ushort n) 567 { 568 const DirectoryEntry *sdp; 569 ASSERT(dp->sub); 570 for (sdp = dp->sub; sdp->name; sdp++) 571 if (strcmp(sdp->name, "..") != 0) { 572 if (n == 0) 573 return sdp; 574 n--; 575 } 576 return 0; 577 } 578 579 int 580 fidwalk(Fid *fp, char name[28]) 581 { 582 const DirectoryEntry *sdp; 583 const DirectoryEntry *dp; 584 585 if (fp->open) 586 return -1; 587 ASSERT(fp->qid[0] < QID_MAP_MAX); 588 dp = qid_map[fp->qid[0]]; 589 if (dp->sub == 0) 590 return -1; 591 for (sdp = dp->sub; sdp->name; sdp++) 592 if (strcmp(sdp->name, name) == 0) { 593 fp->qid[0] = sdp->qid; 594 fp->qid[3] = sdp->sub ? 0x80 : 0; 595 return 1; 596 } 597 return 0; 598 } 599 600 void 601 mkdirent(const DirectoryEntry *dp, uchar *dir) 602 { 603 memset(dir, 0, DIRLEN); 604 strcpy(dir, dp->name); 605 strcpy(dir + 28, "lego"); 606 strcpy(dir + 56, "lego"); 607 dir[84] = dp->qid; 608 dir[92] = dp->sub ? 0555 : 0666; 609 dir[93] = dp->sub ? (0555 >> 8) : (0666 >> 8); 610 dir[95] = dp->sub ? 0x80 : 0; 611 } 612 613 int 614 fidstat(Fid *fp, uchar *dir) 615 { 616 const DirectoryEntry *dp; 617 if (fp->open) 618 return -1; 619 ASSERT(fp->qid[0] < QID_MAP_MAX); 620 dp = qid_map[fp->qid[0]]; 621 mkdirent(dp, dir); 622 return 1; 623 } 624 625 int 626 fidopen(Fid *fp, uchar mode) 627 { 628 if (fp->open 629 || (mode & ORCLOSE) 630 /*|| (mode & OTRUNC) */) 631 return 0; 632 if (fp->qid[3] && (mode == OWRITE || mode == ORDWR)) 633 /* can't write directories */ 634 return 0; 635 fp->open = 1; 636 return 1; 637 } 638 639 short 640 fidread(Fid *fp, ushort tag, ushort offset, ushort count) 641 { 642 short k; 643 uchar *p; 644 const DirectoryEntry *dp; 645 uchar *buf; 646 647 ASSERT(fp->qid[0] < QID_MAP_MAX); 648 dp = qid_map[fp->qid[0]]; 649 650 if (fp->qid[3] & 0x80) { 651 if (!fp->open) 652 return -1; 653 if (count % DIRLEN != 0 || offset % DIRLEN != 0) 654 return -1; 655 count /= DIRLEN; 656 offset /= DIRLEN; 657 buf = send_fid_reply_payload(); 658 p = buf + 3; 659 for (k = 0; k < count; k++) { 660 const DirectoryEntry *sdp = nthentry(dp, offset + k); 661 if (sdp == 0) 662 break; 663 mkdirent(sdp, p); 664 p += DIRLEN; 665 } 666 /* a read beyond just returns 0 667 if (k == 0 && count) 668 return -1; 669 */ 670 k *= DIRLEN; 671 buf[0] = k; 672 buf[1] = k >> 8; 673 buf[2] = 0; 674 send_fid_reply(Rread, tag, fp->fid, 0, k + 3); 675 return 0; 676 } 677 /* right, that's that out of the way */ 678 if (!dp->read) 679 return -1; 680 return (*dp->read)(dp, tag, fp->fid, offset, count); 681 } 682 683 short 684 fidwrite(Fid *fp, ushort offset, ushort count, uchar *buf) 685 { 686 const DirectoryEntry *dp; 687 if (fp->qid[3] & 0x80) 688 return -1; /* can't write directories */ 689 if (!fp->open) 690 return -1; 691 ASSERT(fp->qid[0] < QID_MAP_MAX); 692 dp = qid_map[fp->qid[0]]; 693 if (!dp->write) 694 return -1; /* no write method */ 695 return (*dp->write)(dp, offset, count, buf); 696 } 697 698 int 699 rlencode(unsigned char *out, int limit, unsigned char *in, int len) 700 { 701 unsigned char *ip, *op; 702 int oc, zc; 703 704 if (len == 0) 705 return -1; 706 ip = in; 707 op = out; 708 zc = 0; 709 710 oc = 0; 711 712 for (;;) { 713 int last = ip >= in + len; 714 if (*ip != 0 || last) 715 { 716 switch (zc) { 717 case 1: 718 if (oc >= len - 1) 719 return -1; 720 *op++ = 0; 721 oc++; 722 break; 723 case 2: 724 if (oc >= len - 2) 725 return -1; 726 *op++ = 0; 727 *op++ = 0; 728 oc += 2; 729 break; 730 case 0: 731 break; 732 default: 733 if (oc >= len - 2) 734 return -1; 735 *op++ = 0x88; 736 *op++ = zc - 2; 737 oc += 2; 738 break; 739 } 740 zc = 0; 741 } 742 if (last) 743 break; 744 if (*ip == 0x88) { 745 if (oc >= len - 2) 746 return -1; 747 *op++ = 0x88; 748 *op++ = 0x00; 749 oc += 2; 750 } 751 else if (*ip == 0x00) 752 { 753 zc++; 754 } 755 else { 756 if (oc >= len - 1) 757 return -1; 758 *op++ = *ip; 759 oc++; 760 } 761 ip++; 762 } 763 return oc; 764 } 765 766 int 767 rldecode(unsigned char *out, unsigned char *in, int len) 768 { 769 int oc, k; 770 771 oc = 0; 772 773 while (len) { 774 if (*in != 0x88) { 775 *out++ = *in++; 776 oc++; 777 len--; 778 continue; 779 } 780 in++; 781 switch (*in) { 782 case 0: 783 *out++ = 0x88; 784 oc++; 785 break; 786 default: 787 k = *in + 2; 788 oc += k; 789 while (k-- > 0) 790 *out++ = 0; 791 } 792 in++; 793 len -= 2; 794 } 795 return oc; 796 } 797 798 void 799 prepare_transmission(void) 800 { 801 if (prepared) 802 return; 803 compressed_reply_len = rlencode(compressed_reply + 3, sizeof(compressed_reply) - 3, reply, reply_len); 804 if (compressed_reply_len < 0) { 805 memcpy(compressed_reply + 3, reply, reply_len); 806 compressed_reply_len = reply_len; 807 compressed_reply[2] = 0x0; 808 } 809 else 810 compressed_reply[2] = LLP_COMPRESSION; 811 if (reader_count) 812 compressed_reply[2] |= LLP_POLL_PERIODIC; 813 compressed_reply[2] |= !alternating_bit; 814 compressed_reply_len++; 815 compressed_reply[0] = compressed_reply_len; 816 compressed_reply[1] = compressed_reply_len >> 8; 817 compressed_reply_len += 2; 818 prepared = 1; 819 } 820 821 void 822 transmit(void) 823 { 824 prepare_transmission(); 825 transmitted_reply_len = reply_len; 826 send_message(compressed_reply, compressed_reply_len); 827 } 828 829 void 830 flush_reply_buffer(void) 831 { 832 if (reply_len > transmitted_reply_len) 833 memcpy(reply, reply + transmitted_reply_len, reply_len - transmitted_reply_len); 834 reply_len -= transmitted_reply_len; 835 prepared = 0; 836 } 837 838 void 839 send_reply(unsigned char type, unsigned short tag, unsigned char *msg, short len) 840 { 841 uchar *p = reply + reply_len; 842 p[0] = type; 843 p[1] = tag & 0xff; 844 p[2] = tag >> 8; 845 if (msg) 846 memcpy(p + 3, msg, len); 847 reply_len += len + 3; 848 prepared = 0; 849 } 850 851 void 852 send_error_reply(unsigned short tag, char *msg) 853 { 854 short len; 855 uchar *p = reply + reply_len; 856 p[0] = Rerror; 857 p[1] = tag & 0xff; 858 p[2] = tag >> 8; 859 len = (short)strlen(msg); 860 if (len > 64) 861 len = 64; 862 memcpy(p + 3, msg, len); 863 reply_len += 67; 864 prepared = 0; 865 } 866 867 uchar * 868 send_fid_reply_payload(void) 869 { 870 return reply + reply_len + 5; 871 } 872 873 void 874 send_fid_reply(uchar type, ushort tag, ushort fid, uchar *msg, short len) 875 { 876 uchar *p = reply + reply_len; 877 p[0] = type; 878 p[1] = tag & 0xff; 879 p[2] = tag >> 8; 880 p[3] = fid & 0xff; 881 p[4] = fid >> 8; 882 if (msg) 883 memcpy(p + 5, msg, len); 884 reply_len += len + 5; 885 prepared = 0; 886 } 887 888 int 889 matchtag(Reader *rp, void *oldtag) 890 { 891 if (rp->tag == (ushort)oldtag) { 892 return 1; 893 } 894 return 0; 895 } 896 897 void 898 flushtag(ushort oldtag) 899 { 900 /* a little inefficient this - there can be at most one match! */ 901 allreaderlistfindanddestroy(matchtag, (void *)oldtag); 902 } 903 904 void 905 process_styx_message(unsigned char *msg, short len) 906 { 907 unsigned char type; 908 ushort tag, oldtag, fid, newfid; 909 ushort offset, count; 910 short extra; 911 Fid *fp, *nfp; 912 short written; 913 uchar buf[2]; 914 915 ASSERT(len >= 3); 916 917 type = *msg++; len--; 918 tag = (msg[1] << 8) | msg[0]; len -= 2; msg += 2; 919 920 switch (type) { 921 case Tnop: 922 send_reply(Rnop, tag, 0, 0); 923 goto done; 924 case Tflush: 925 ASSERT(len == 2); 926 oldtag = (msg[1] << 8) | msg[0]; 927 flushtag(oldtag); 928 send_reply(Rflush, tag, 0, 0); 929 goto done; 930 } 931 /* all other messages take a fid as well */ 932 ASSERT(len >= 2); 933 fid = (msg[1] << 8) | msg[0]; len -= 2; msg += 2; 934 fp = fidfind(fid); 935 936 switch (type) { 937 case Tattach: 938 ASSERT(len == 56); 939 if (fp) { 940 fid_in_use: 941 send_error_reply(tag, "fid in use"); 942 } 943 else { 944 fp = fidcreate(fid, qid_root); 945 send_fid_reply(Rattach, tag, fid, fp->qid, 8); 946 } 947 break; 948 case Tclunk: 949 case Tremove: 950 ASSERT(len == 0); 951 if (!fp) { 952 no_such_fid: 953 send_error_reply(tag, "no such fid"); 954 } 955 else { 956 fiddelete(fp); 957 if (type == Tremove) 958 send_error_reply(tag, "can't remove"); 959 else 960 send_fid_reply(Rclunk, tag, fid, 0, 0); 961 } 962 break; 963 case Tclone: 964 ASSERT(len == 2); 965 newfid = (msg[1] << 8) | msg[0]; 966 nfp = fidfind(newfid); 967 if (!fp) 968 goto no_such_fid; 969 if (fp->open) { 970 send_error_reply(tag, "can't clone"); 971 break; 972 } 973 if (nfp) 974 goto fid_in_use; 975 nfp = fidcreate(newfid, fp->qid); 976 send_fid_reply(Rclone, tag, fid, 0, 0); 977 break; 978 case Twalk: 979 ASSERT(len == 28); 980 if (!fidwalk(fp, msg)) 981 send_error_reply(tag, "no such name"); 982 else 983 send_fid_reply(Rwalk, tag, fid, fp->qid, 8); 984 break; 985 case Tstat: 986 ASSERT(len == 0); 987 if (!fidstat(fp, dir)) 988 send_error_reply(tag, "can't stat"); 989 else 990 send_fid_reply(Rstat, tag, fid, dir, 116); 991 break; 992 ASSERT(len == 0); 993 case Tcreate: 994 ASSERT(len == 33); 995 send_error_reply(tag, "can't create"); 996 break; 997 case Topen: 998 ASSERT(len == 1); 999 if (!fidopen(fp, msg[0])) 1000 send_error_reply(tag, "can't open"); 1001 else 1002 send_fid_reply(Ropen, tag, fid, fp->qid, 8); 1003 break; 1004 case Tread: 1005 ASSERT(len == 10); 1006 offset = (msg[1] << 8) | msg[0]; 1007 count = (msg[9] << 8) | msg[8]; 1008 if (fidread(fp, tag, offset, count) < 0) 1009 send_error_reply(tag, "can't read"); 1010 break; 1011 case Twrite: 1012 ASSERT(len >= 11); 1013 offset = (msg[1] << 8) | msg[0]; 1014 count = (msg[9] << 8) | msg[8]; 1015 msg += 11; 1016 len -= 11; 1017 ASSERT(count == len); 1018 written = fidwrite(fp, offset, count, msg); 1019 if (written < 0) 1020 send_error_reply(tag, "can't write"); 1021 else { 1022 buf[0] = written; 1023 buf[1] = written >> 8; 1024 send_fid_reply(Rwrite, tag, fid, buf, 2); 1025 } 1026 break; 1027 default: 1028 FATAL; 1029 } 1030 done: 1031 ; 1032 } 1033 1034 void 1035 process_llp_message(unsigned char *msg, short len) 1036 { 1037 short styxlen; 1038 switch (msg[0]) { 1039 case 0x45: 1040 case 0x4d: 1041 if (len != 5) 1042 FATAL; 1043 styxlen = compressed_incoming[0] | (compressed_incoming[1] << 8); 1044 /* transfer the transmitted checksum to the end */ 1045 compressed_incoming[styxlen + 2 - 1] = msg[3]; 1046 /* check alternating bit */ 1047 #ifdef ABP 1048 if ((compressed_incoming[2] & 1) != alternating_bit || 1049 ((msg[0] & 8) != 0) != alternating_bit) { 1050 transmit(); 1051 break; 1052 } 1053 #endif 1054 alternating_bit = !alternating_bit; 1055 flush_reply_buffer(); 1056 if (styxlen > 1) { 1057 if (compressed_incoming[2] & LLP_COMPRESSION) { 1058 /* decompress everything but length and link header */ 1059 styxlen = rldecode(incoming, compressed_incoming + 3, styxlen - 1); 1060 process_styx_message(incoming, styxlen); 1061 } 1062 else 1063 process_styx_message(compressed_incoming + 3, styxlen - 1); 1064 } 1065 transmit(); 1066 break; 1067 default: 1068 FATAL; 1069 } 1070 } 1071 1072 int 1073 main (void) 1074 { 1075 int count = 0; 1076 char buf[16]; 1077 char temp[64]; 1078 1079 mem_init(); 1080 memset(temp,0, sizeof(temp)); 1081 1082 /* Initialize */ 1083 1084 init_timer(&temp[6], &dispatch[0]); 1085 init_power(); 1086 init_sensors(); 1087 init_serial(&temp[4], &temp[6], 1, 1); 1088 1089 set_lcd_number(LCD_UNSIGNED, 0, LCD_DECIMAL_0); 1090 set_lcd_segment(LCD_WALKING); 1091 refresh_display(); 1092 1093 set_data_pointer(compressed_incoming); 1094 1095 alternating_bit = 0; 1096 compressed_reply_len = 0; 1097 reply_len = 0; 1098 prepared = 0; 1099 1100 while (poll_power() != 0) { 1101 1102 /* If a message has arrived, send a response with opcode inverted */ 1103 1104 if (check_valid()) { 1105 int len = receive_message(buf, sizeof(buf)); 1106 msgcount++; 1107 process_llp_message(buf, len); 1108 } 1109 sensorpoll(); 1110 } 1111 1112 return 0; 1113 } 1114