aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/Source')
-rw-r--r--AT91SAM7S256/Source/BtTest.inc1661
-rw-r--r--AT91SAM7S256/Source/Connections.txt23
-rw-r--r--AT91SAM7S256/Source/Cursor.txt13
-rw-r--r--AT91SAM7S256/Source/Devices.txt23
-rw-r--r--AT91SAM7S256/Source/Display.txt14
-rw-r--r--AT91SAM7S256/Source/Fail.txt14
-rw-r--r--AT91SAM7S256/Source/Font.txt17
-rw-r--r--AT91SAM7S256/Source/Functions.inl4331
-rw-r--r--AT91SAM7S256/Source/Icons.txt293
-rw-r--r--AT91SAM7S256/Source/Info.txt14
-rw-r--r--AT91SAM7S256/Source/LowBattery.txt18
-rw-r--r--AT91SAM7S256/Source/Mainmenu.rms72
-rw-r--r--AT91SAM7S256/Source/Ok.txt13
-rw-r--r--AT91SAM7S256/Source/Port.txt12
-rw-r--r--AT91SAM7S256/Source/RCXintro_1.txt19
-rw-r--r--AT91SAM7S256/Source/RCXintro_10.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_11.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_12.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_13.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_14.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_15.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_16.txt13
-rw-r--r--AT91SAM7S256/Source/RCXintro_2.txt19
-rw-r--r--AT91SAM7S256/Source/RCXintro_3.txt19
-rw-r--r--AT91SAM7S256/Source/RCXintro_4.txt19
-rw-r--r--AT91SAM7S256/Source/RCXintro_5.txt19
-rw-r--r--AT91SAM7S256/Source/RCXintro_6.txt17
-rw-r--r--AT91SAM7S256/Source/RCXintro_7.txt16
-rw-r--r--AT91SAM7S256/Source/RCXintro_8.txt14
-rw-r--r--AT91SAM7S256/Source/RCXintro_9.txt14
-rw-r--r--AT91SAM7S256/Source/Running.txt59
-rw-r--r--AT91SAM7S256/Source/Status.txt17
-rw-r--r--AT91SAM7S256/Source/Step.txt19
-rw-r--r--AT91SAM7S256/Source/Submenu01.rms128
-rw-r--r--AT91SAM7S256/Source/Submenu02.rms401
-rw-r--r--AT91SAM7S256/Source/Submenu03.rms233
-rw-r--r--AT91SAM7S256/Source/Submenu04.rms163
-rw-r--r--AT91SAM7S256/Source/Submenu05.rms128
-rw-r--r--AT91SAM7S256/Source/Submenu06.rms51
-rw-r--r--AT91SAM7S256/Source/Submenu07.rms142
-rw-r--r--AT91SAM7S256/Source/Test1.txt19
-rw-r--r--AT91SAM7S256/Source/Test2.txt19
-rw-r--r--AT91SAM7S256/Source/Ui.txt72
-rw-r--r--AT91SAM7S256/Source/Wait.txt14
-rw-r--r--AT91SAM7S256/Source/c_button.c134
-rw-r--r--AT91SAM7S256/Source/c_button.h37
-rw-r--r--AT91SAM7S256/Source/c_button.iom61
-rw-r--r--AT91SAM7S256/Source/c_cmd.c7992
-rw-r--r--AT91SAM7S256/Source/c_cmd.h882
-rw-r--r--AT91SAM7S256/Source/c_cmd.iom202
-rw-r--r--AT91SAM7S256/Source/c_cmd_bytecodes.h119
-rw-r--r--AT91SAM7S256/Source/c_cmd_drawing.inc894
-rw-r--r--AT91SAM7S256/Source/c_comm.c3712
-rw-r--r--AT91SAM7S256/Source/c_comm.h154
-rw-r--r--AT91SAM7S256/Source/c_comm.iom223
-rw-r--r--AT91SAM7S256/Source/c_display.c849
-rw-r--r--AT91SAM7S256/Source/c_display.h43
-rw-r--r--AT91SAM7S256/Source/c_display.iom177
-rw-r--r--AT91SAM7S256/Source/c_input.c1523
-rw-r--r--AT91SAM7S256/Source/c_input.h67
-rw-r--r--AT91SAM7S256/Source/c_input.iom175
-rw-r--r--AT91SAM7S256/Source/c_ioctrl.c78
-rw-r--r--AT91SAM7S256/Source/c_ioctrl.h28
-rw-r--r--AT91SAM7S256/Source/c_ioctrl.iom35
-rw-r--r--AT91SAM7S256/Source/c_loader.c464
-rw-r--r--AT91SAM7S256/Source/c_loader.h44
-rw-r--r--AT91SAM7S256/Source/c_loader.iom84
-rw-r--r--AT91SAM7S256/Source/c_lowspeed.c236
-rw-r--r--AT91SAM7S256/Source/c_lowspeed.h61
-rw-r--r--AT91SAM7S256/Source/c_lowspeed.iom95
-rw-r--r--AT91SAM7S256/Source/c_output.c169
-rw-r--r--AT91SAM7S256/Source/c_output.h31
-rw-r--r--AT91SAM7S256/Source/c_output.iom92
-rw-r--r--AT91SAM7S256/Source/c_sound.c309
-rw-r--r--AT91SAM7S256/Source/c_sound.h44
-rw-r--r--AT91SAM7S256/Source/c_sound.iom109
-rw-r--r--AT91SAM7S256/Source/c_ui.c1944
-rw-r--r--AT91SAM7S256/Source/c_ui.h387
-rw-r--r--AT91SAM7S256/Source/c_ui.iom127
-rw-r--r--AT91SAM7S256/Source/config.h10
-rw-r--r--AT91SAM7S256/Source/d_bt.c452
-rw-r--r--AT91SAM7S256/Source/d_bt.h39
-rw-r--r--AT91SAM7S256/Source/d_bt.r347
-rw-r--r--AT91SAM7S256/Source/d_button.c36
-rw-r--r--AT91SAM7S256/Source/d_button.h24
-rw-r--r--AT91SAM7S256/Source/d_button.r189
-rw-r--r--AT91SAM7S256/Source/d_display.c53
-rw-r--r--AT91SAM7S256/Source/d_display.h39
-rw-r--r--AT91SAM7S256/Source/d_display.r374
-rw-r--r--AT91SAM7S256/Source/d_hispeed.c48
-rw-r--r--AT91SAM7S256/Source/d_hispeed.h25
-rw-r--r--AT91SAM7S256/Source/d_hispeed.r190
-rw-r--r--AT91SAM7S256/Source/d_input.c152
-rw-r--r--AT91SAM7S256/Source/d_input.h48
-rw-r--r--AT91SAM7S256/Source/d_input.r312
-rw-r--r--AT91SAM7S256/Source/d_ioctrl.c47
-rw-r--r--AT91SAM7S256/Source/d_ioctrl.h25
-rw-r--r--AT91SAM7S256/Source/d_ioctrl.r237
-rw-r--r--AT91SAM7S256/Source/d_loader.c1480
-rw-r--r--AT91SAM7S256/Source/d_loader.h109
-rw-r--r--AT91SAM7S256/Source/d_loader.r117
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.c77
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.h28
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.r954
-rw-r--r--AT91SAM7S256/Source/d_output.c1616
-rw-r--r--AT91SAM7S256/Source/d_output.h90
-rw-r--r--AT91SAM7S256/Source/d_output.r306
-rw-r--r--AT91SAM7S256/Source/d_sound.c70
-rw-r--r--AT91SAM7S256/Source/d_sound.h42
-rw-r--r--AT91SAM7S256/Source/d_sound.r515
-rw-r--r--AT91SAM7S256/Source/d_sound_adpcm.r158
-rw-r--r--AT91SAM7S256/Source/d_timer.c62
-rw-r--r--AT91SAM7S256/Source/d_timer.h32
-rw-r--r--AT91SAM7S256/Source/d_timer.r76
-rw-r--r--AT91SAM7S256/Source/d_usb.c946
-rw-r--r--AT91SAM7S256/Source/d_usb.h51
-rw-r--r--AT91SAM7S256/Source/d_usb.r65
-rw-r--r--AT91SAM7S256/Source/m_sched.c94
-rw-r--r--AT91SAM7S256/Source/m_sched.h135
-rw-r--r--AT91SAM7S256/Source/modules.h338
-rw-r--r--AT91SAM7S256/Source/stdconst.h74
121 files changed, 39893 insertions, 0 deletions
diff --git a/AT91SAM7S256/Source/BtTest.inc b/AT91SAM7S256/Source/BtTest.inc
new file mode 100644
index 0000000..f879e20
--- /dev/null
+++ b/AT91SAM7S256/Source/BtTest.inc
@@ -0,0 +1,1661 @@
+//******* TestPrg ************************************************************
+
+//#define TESTPRG // If defined the test program will be included
+
+#ifdef TESTPRG
+#include "Test1.txt"
+#include "Test2.txt"
+#endif
+
+extern void BtIo(void);
+
+const char BUILD_DATE[] = __DATE__;
+const char BUILD_TIME[] = __TIME__;
+
+const char MONTH[] = "JanFebMarAprMayJunJulAugSepOctNovDec";
+
+void GetProtocolVersion(UBYTE *String)
+{
+ UWORD Tmp;
+
+ Tmp = FIRMWAREVERSION & 0x00FF;
+
+ if (Tmp < 100)
+ {
+#ifdef CUSTOM_FIRMWAREVERSION
+ int pad = (sizeof (CUSTOM_FIRMWAREVERSION) - 1) > 7 ? 1 : 1 + 7 - (sizeof (CUSTOM_FIRMWAREVERSION) - 1);
+ sprintf((char*)String,"FW %*u.%02ui-%.7s", pad, (FIRMWAREVERSION >> 8) & 0x00FF,Tmp & 0x00FF, CUSTOM_FIRMWAREVERSION);
+#else
+ sprintf((char*)String,"FW %3u.%02u",(FIRMWAREVERSION >> 8) & 0x00FF,Tmp & 0x00FF);
+#endif
+ }
+ else
+ {
+ sprintf((char*)String,"FW Hex %2X.%02X",(FIRMWAREVERSION >> 8) & 0x00FF,Tmp & 0x00FF);
+ }
+}
+
+
+void GetARMBuild(UBYTE *String)
+{
+ UWORD Tmp;
+ UWORD Lng;
+ char String1[4];
+ char String2[4];
+
+ String1[0] = BUILD_DATE[4];
+ String1[1] = BUILD_DATE[5];
+ String1[2] = 0;
+
+ Tmp = (UWORD)atoi(String1);
+ Lng = 0;
+ Lng += sprintf((char*)&String[Lng],"BUILD ");
+ Lng += sprintf((char*)&String[Lng],"%02u",Tmp);
+
+ String1[0] = BUILD_DATE[0];
+ String1[1] = BUILD_DATE[1];
+ String1[2] = BUILD_DATE[2];
+ String1[3] = 0;
+ String2[3] = 0;
+
+ Tmp = 0;
+ do
+ {
+ String2[0] = MONTH[0 + 3 * Tmp];
+ String2[1] = MONTH[1 + 3 * Tmp];
+ String2[2] = MONTH[2 + 3 * Tmp];
+ Tmp++;
+ }
+ while ((Tmp < 12) && (strcmp(String1,String2) != 0));
+
+ Lng += sprintf((char*)&String[Lng],"%02u",Tmp);
+ Lng += sprintf((char*)&String[Lng],"%c%c",BUILD_DATE[9],BUILD_DATE[10]);
+ Lng += sprintf((char*)&String[Lng],"%c%c",BUILD_TIME[0],BUILD_TIME[1]);
+ Lng += sprintf((char*)&String[Lng],"%c%c",BUILD_TIME[3],BUILD_TIME[4]);
+}
+
+
+void GetBC4Build(UBYTE *String)
+{
+ sprintf((char*)String,"BC4 %2X.%02X",pMapComm->BrickData.BluecoreVersion[1],pMapComm->BrickData.BluecoreVersion[0]);
+}
+
+
+void GetAVRBuild(UBYTE *String)
+{
+ sprintf((char*)String,"AVR %1u.%02u",((IoFromAvr.Battery >> 13) & 3),((IoFromAvr.Battery >> 10) & 7));
+}
+
+
+void GetBC4Address(UBYTE *String)
+{
+ UWORD Count;
+ UBYTE Tmp;
+
+ Count = (UWORD)sprintf((char*)String,"ID ");
+ for (Tmp = 0;(Tmp < (SIZE_OF_BDADDR - 1)) && (Count <= (DISPLAYLINE_LENGTH - 2));Tmp++)
+ {
+ Count += (UWORD)sprintf((char*)&String[Count],"%02X",(UWORD)(pMapComm->BrickData.BdAddr[Tmp]) & 0xFF);
+ }
+}
+
+
+enum TSTPRG
+{
+ SYSTEM_INIT = 1,
+ SYSTEM_UNLOCK_INIT,
+ SYSTEM_UNLOCK,
+ SYSTEM_PAGE,
+ TIMER_INIT,
+ TIMER_SHOW,
+ TIMER_HOLD,
+ BT_PAGE,
+ BT_RESET,
+ BT_RESETTING,
+ BT_LIST_INIT,
+ BT_LIST,
+ BT_CONN_INIT,
+ BT_CONN,
+ BT_UPDATE_FW,
+ TSTPRG_INIT,
+ TSTPRG_SELECT_SUBTEST,
+
+ TSTPRG_SENSOR_INIT,
+ TSTPRG_SELECT_SENSOR,
+ TSTPRG_TOUCH_SENSOR_INIT,
+ TSTPRG_TOUCH_SENSOR,
+ TSTPRG_SOUND_SENSOR_SELECT,
+ TSTPRG_SOUND_SENSOR_INIT,
+ TSTPRG_SOUND_SENSOR,
+ TSTPRG_LIGHT_SENSOR_SELECT,
+ TSTPRG_LIGHT_SENSOR_INIT,
+ TSTPRG_LIGHT_SENSOR,
+ TSTPRG_SKIP_SENSOR,
+
+ TSTPRG_RCX_INIT,
+ TSTPRG_RCX_SELECT,
+ TSTPRG_RCX_DISPLAY_INIT,
+ TSTPRG_RCX_DISPLAY,
+ TSTPRG_RCX_INPUT_SELECT,
+ TSTPRG_RCX_INPUT_INIT,
+ TSTPRG_RCX_INPUT,
+ TSTPRG_RCX_DIGITAL_INIT,
+ TSTPRG_RCX_DIGITAL_OK,
+ TSTPRG_RCX_DIGITAL_FAIL,
+ TSTPRG_RCX_DIGITAL,
+ TSTPRG_RCX_MOTOR_INIT,
+ TSTPRG_RCX_MOTOR,
+ TSTPRG_SKIP_RCX_MOTOR,
+ TSTPRG_SKIP_RCX,
+
+ TSTPRG_MOTOR_INIT,
+ TSTPRG_MOTOR,
+ TSTPRG_SKIP_MOTOR,
+
+ TSTPRG_SKIP,
+ TSTPRG_WAIT
+};
+
+const UBYTE TXT_EMPTY[] = " ";
+const UBYTE TXT_LINE[] = "----------------";
+
+#ifdef TESTPRG
+
+const UBYTE TXT_TEST[] = "Timer Test Bt ";
+
+const UBYTE TXT_TIMER[] = "Reset Hold ";
+const UBYTE TXT_TIMER_HOLD[] = " Continue ";
+
+const UBYTE TXT_LAST[] = "Last UI->BT Cmd.";
+const UBYTE TXT_BT_PAGE[] = "Reset List BtIo";
+
+const UBYTE TXT_RESETTING[] = " Resetting! ";
+
+const UBYTE TXT_BT_LIST[] = "Down ConTab Up ";
+const UBYTE TXT_BT_CONN[] = "Down Update Up ";
+
+const UBYTE TXT_BTUPDATE[] = "BT update mode !";
+const UBYTE TXT_DONE[] = " When done ";
+const UBYTE TXT_RESET[] = " activate reset ";
+const UBYTE TXT_REBOOT[] = "button to reboot";
+
+const UBYTE TXT_TESTPRG[] = " TestPrg V0.08 ";
+const UBYTE TXT_SELECT[] = "Select sub test ";
+const UBYTE TXT_SUBTEST[] = "Sens. RCX Motor";
+
+const UBYTE TXT_SELECT_SENSOR[] = " Select sensor ";
+const UBYTE TXT_SENSORS[] = "Touch Snd. Light";
+
+const UBYTE TXT_SELECT_TYPE[] = " Select type ";
+const UBYTE TXT_SOUND_SENSORS[] = " DB DBA ";
+const UBYTE TXT_LIGHT_SENSORS[] = "Pasive Active";
+
+const UBYTE TXT_SENSOR_TOUCH[] = "Touch Sensor Tst";
+const UBYTE TXT_SENSOR_SOUND_DB[] = "DB Sound Sens.";
+const UBYTE TXT_SENSOR_SOUND_DBA[] = "DBA Sound Sens.";
+const UBYTE TXT_SOUND_STOP[] = "440Hz Stop 4KHz";
+const UBYTE TXT_SENSOR_LIGHT_PASIVE[] = "Pas. Light Sens.";
+const UBYTE TXT_SENSOR_LIGHT_ACTIVE[] = "Act. Light Sens.";
+
+const UBYTE TXT_SUBTEST_STOP[] = " Stop ";
+
+const UBYTE TXT_MOTOR[] = " Motor test ";
+const UBYTE TXT_MOTOR_HEADER[] = "Outp Set Cnt";
+const UBYTE TXT_MOTOR_STOP[] = "Bwd Stop Fwd";
+
+const UBYTE TXT_RCX[] = " RCX test ";
+const UBYTE TXT_RCX_STOP[] = "Inp Disp Outp";
+const UBYTE TXT_RCX_INPUT_PASIVE[] = "Input pasive Tst";
+const UBYTE TXT_RCX_INPUT_ACTIVE[] = "Input active Tst";
+const UBYTE TXT_RCX_INPUT_SELECT[] = "Pas. Act. Dig.";
+const UBYTE TXT_RCX_INPUT_DIGITAL[] = "Digital I/O Tst";
+const UBYTE TXT_RCX_DIGITAL_OK[] = " OK ";
+const UBYTE TXT_RCX_DIGITAL_FAIL[] = " FAIL ";
+const UBYTE TXT_MOTOR_NEXT[] = "Bwd Next Fwd";
+
+
+void TestPrgRunMotor(UBYTE No,SBYTE Speed)
+{
+ pMapOutPut->Outputs[No].Mode = MOTORON | BRAKE;
+ pMapOutPut->Outputs[No].Speed = Speed;
+ pMapOutPut->Outputs[No].TachoLimit = 0;
+ pMapOutPut->Outputs[No].RunState = MOTOR_RUN_STATE_RUNNING;
+ pMapOutPut->Outputs[No].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
+}
+
+void TestPrgFloatMotor(UBYTE No)
+{
+ pMapOutPut->Outputs[No].Mode = 0;
+ pMapOutPut->Outputs[No].Speed = 0;
+ pMapOutPut->Outputs[No].TachoLimit = 0;
+ pMapOutPut->Outputs[No].RunState = MOTOR_RUN_STATE_RUNNING;
+ pMapOutPut->Outputs[No].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
+}
+
+SBYTE TestPrgReadMotor(UBYTE No)
+{
+ return ((SBYTE)(pMapOutPut->Outputs[No].TachoCnt / 360));
+}
+
+#endif
+
+
+UBYTE TestPrg(UBYTE Dummy)
+{
+ static UWORD Count;
+ static UBYTE TxtBuffer[TEXTLINES][DISPLAYLINE_LENGTH + 1];
+ static UBYTE State = SYSTEM_INIT;
+#ifdef TESTPRG
+ static UWORD Pointer;
+ static UWORD InputValues[NO_OF_INPUTS];
+ static SWORD OutputValues[NO_OF_OUTPUTS];
+ static UBYTE VolumeSave;
+ static UBYTE Timer;
+ static UBYTE SubState = 0;
+ UBYTE Tmp;
+#endif
+
+ Dummy = Dummy;
+ switch (State)
+ {
+ case SYSTEM_INIT :
+ {
+ GetProtocolVersion(TxtBuffer[0]);
+ GetAVRBuild(TxtBuffer[1]);
+ GetBC4Build(TxtBuffer[2]);
+ GetARMBuild(TxtBuffer[3]);
+ GetBC4Address(TxtBuffer[4]);
+
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TxtBuffer[0];
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TxtBuffer[1];
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TxtBuffer[2];
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TxtBuffer[3];
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TxtBuffer[4];
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4) | TEXTLINE_BIT(TEXTLINE_5) | TEXTLINE_BIT(TEXTLINE_6) | TEXTLINE_BIT(TEXTLINE_7) | TEXTLINE_BIT(TEXTLINE_8));
+
+#ifdef TESTPRG
+ SubState = 0;
+#endif
+ State = SYSTEM_UNLOCK_INIT;
+ }
+ break;
+
+#ifndef TESTPRG
+
+ case SYSTEM_UNLOCK_INIT : // ENTER * 1 + LEFT * 3 + RIGHT * 2 + ENTER * 1 = TEST MENU
+ {
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP;
+ }
+ }
+ break;
+
+#else
+
+ case SYSTEM_UNLOCK_INIT : // ENTER * 1 + LEFT * 3 + RIGHT * 2 + ENTER * 1 = TEST MENU
+ {
+ Tmp = cUiReadButtons();
+ switch (Tmp)
+ {
+ case BUTTON_ENTER :
+ {
+ switch (SubState)
+ {
+ case 0 :
+ {
+ SubState = 1;
+ Pointer = 0;
+ Count = 0;
+ }
+ break;
+
+ case 3 :
+ {
+ State = SYSTEM_UNLOCK;
+ }
+ break;
+
+ default :
+ {
+ Tmp = BUTTON_EXIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case BUTTON_NONE :
+ {
+ }
+ break;
+
+ default :
+ {
+ switch (SubState)
+ {
+ case 0 :
+ {
+ Tmp = BUTTON_EXIT;
+ }
+ break;
+
+ case 1 :
+ {
+ if (Tmp == BUTTON_LEFT)
+ {
+ if (++Count >= 3)
+ {
+ Count = 0;
+ SubState = 2;
+ }
+ Pointer = 0;
+ }
+ else
+ {
+ Tmp = BUTTON_EXIT;
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ if (Tmp == BUTTON_RIGHT)
+ {
+ if (++Count >= 2)
+ {
+ SubState = 3;
+ }
+ Pointer = 0;
+ }
+ else
+ {
+ Tmp = BUTTON_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+ Pointer++;
+ if (((SubState) && (Pointer > 500)) || (Tmp == BUTTON_EXIT))
+ {
+ State = TSTPRG_SKIP;
+ }
+ }
+ break;
+
+ case SYSTEM_UNLOCK :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_TEST;
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_8);
+ State = SYSTEM_PAGE;
+ }
+ break;
+
+ case SYSTEM_PAGE :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ IOMapUi.Flags &= ~UI_ENABLE_STATUS_UPDATE;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ State = TSTPRG_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ Count = 0;
+ State = TSTPRG_SKIP;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ IOMapUi.Flags &= ~UI_ENABLE_STATUS_UPDATE;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_TIMER;
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TIMER_INIT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ IOMapUi.Flags &= ~UI_ENABLE_STATUS_UPDATE;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_BT_PAGE;
+ if (DISPLAYLINE_LENGTH >= 16)
+ {
+ sprintf((char*)TxtBuffer[2],"Command %02X",(UWORD)VarsUi.BTCommand & 0xFF);
+ sprintf((char*)TxtBuffer[3],"Parameter 1 %02X",(UWORD)VarsUi.BTPar1 & 0xFF);
+ sprintf((char*)TxtBuffer[4],"Parameter 2 %02X",(UWORD)VarsUi.BTPar2 & 0xFF);
+ sprintf((char*)TxtBuffer[5],"Result %04X",(UWORD)VarsUi.BTResult);
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_LAST;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = TxtBuffer[2];
+ pMapDisplay->pTextLines[TEXTLINE_4] = TxtBuffer[3];
+ pMapDisplay->pTextLines[TEXTLINE_5] = TxtBuffer[4];
+ pMapDisplay->pTextLines[TEXTLINE_6] = TxtBuffer[5];
+ }
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = BT_PAGE;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TIMER_INIT :
+ {
+ State = TIMER_SHOW;
+ }
+ break;
+
+ case TIMER_SHOW :
+ {
+ sprintf((char*)TxtBuffer[2]," %10lu mS ",VarsUi.CRPasskey);
+ pMapDisplay->pTextLines[TEXTLINE_3] = TxtBuffer[2];
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_TIMER_HOLD;
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_8);
+ State = TIMER_HOLD;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_3);
+ VarsUi.CRPasskey = 0L;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TIMER_HOLD :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_TIMER;
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_8);
+ State = TIMER_SHOW;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case BT_PAGE :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ for (Count = 0;Count < TEXTLINES;Count++)
+ {
+ strcpy((char*)TxtBuffer[Count],(char*)TXT_EMPTY);
+ pMapDisplay->pTextLines[TEXTLINE_1 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_BT_LIST;
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ Pointer = 0;
+ State = BT_LIST_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ Count = 0;
+ State = TSTPRG_SKIP;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ State = BT_RESET;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_BTUPDATE;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_DONE;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_RESET;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_REBOOT;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ Timer = 0;
+ State = BT_UPDATE_FW;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case BT_RESET :
+ {
+ VarsUi.BTCommand = (UBYTE)FACTORYRESET;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_RESETTING;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = BT_RESETTING;
+ }
+ else
+ {
+ State = TSTPRG_SKIP;
+ }
+ }
+ break;
+
+ case BT_RESETTING :
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ State = TSTPRG_SKIP;
+ }
+ }
+ break;
+
+ case BT_UPDATE_FW :
+ {
+ if (++Timer >= 100)
+ {
+ BtIo();
+ }
+ }
+ break;
+
+ case BT_LIST_INIT :
+ {
+ sprintf((char*)TxtBuffer[0],"DeviceTable No%2u",Pointer);
+ sprintf((char*)TxtBuffer[2],"%-*.*s",DISPLAYLINE_LENGTH,DISPLAYLINE_LENGTH,(char*)pMapComm->BtDeviceTable[Pointer].Name);
+ Count = (UWORD)sprintf((char*)TxtBuffer[3],"COD=");
+ for (Tmp = 0;(Tmp < SIZE_OF_CLASS_OF_DEVICE) && (Count < (DISPLAYLINE_LENGTH - 2));Tmp++)
+ {
+ Count += (UWORD)sprintf((char*)&TxtBuffer[3][Count],"%02X",(UWORD)(pMapComm->BtDeviceTable[Pointer].ClassOfDevice[Tmp]) & 0xFF);
+ }
+ Count = (UWORD)sprintf((char*)TxtBuffer[4],"A=");
+ for (Tmp = 0;(Tmp < SIZE_OF_BDADDR) && (Count <= (DISPLAYLINE_LENGTH - 2));Tmp++)
+ {
+ Count += (UWORD)sprintf((char*)&TxtBuffer[4][Count],"%02X",(UWORD)(pMapComm->BtDeviceTable[Pointer].BdAddr[Tmp]) & 0xFF);
+ }
+ sprintf((char*)TxtBuffer[5],"Status=%02X",(UWORD)(pMapComm->BtDeviceTable[Pointer].DeviceStatus) & 0xFF);
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = BT_LIST;
+ }
+ break;
+
+ case BT_LIST :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ for (Count = 0;Count < TEXTLINES;Count++)
+ {
+ strcpy((char*)TxtBuffer[Count],(char*)TXT_EMPTY);
+ pMapDisplay->pTextLines[TEXTLINE_1 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_BT_CONN;
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ Pointer = 0;
+ State = BT_CONN_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = SYSTEM_INIT;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ if (Pointer)
+ {
+ Pointer--;
+ }
+ else
+ {
+ Pointer = (SIZE_OF_BT_DEVICE_TABLE - 1);
+ }
+ State = BT_LIST_INIT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ if (Pointer < (SIZE_OF_BT_DEVICE_TABLE - 1))
+ {
+ Pointer++;
+ }
+ else
+ {
+ Pointer = 0;
+ }
+ State = BT_LIST_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case BT_CONN_INIT :
+ {
+ sprintf((char*)TxtBuffer[0],"Conn. Table No%2u",Pointer);
+ sprintf((char*)TxtBuffer[2],"%-*.*s",DISPLAYLINE_LENGTH,DISPLAYLINE_LENGTH,(char*)pMapComm->BtConnectTable[Pointer].Name);
+ Count = (UWORD)sprintf((char*)TxtBuffer[3],"COD=");
+ for (Tmp = 0;(Tmp < SIZE_OF_CLASS_OF_DEVICE) && (Count < (DISPLAYLINE_LENGTH - 2));Tmp++)
+ {
+ Count += (UWORD)sprintf((char*)&TxtBuffer[3][Count],"%02X",(UWORD)(pMapComm->BtConnectTable[Pointer].ClassOfDevice[Tmp]) & 0xFF);
+ }
+ Count = (UWORD)sprintf((char*)TxtBuffer[4],"A=");
+ for (Tmp = 0;(Tmp < SIZE_OF_BDADDR) && (Count <= (DISPLAYLINE_LENGTH - 2));Tmp++)
+ {
+ Count += (UWORD)sprintf((char*)&TxtBuffer[4][Count],"%02X",(UWORD)(pMapComm->BtConnectTable[Pointer].BdAddr[Tmp]) & 0xFF);
+ }
+ sprintf((char*)TxtBuffer[5],"%-*.*s",DISPLAYLINE_LENGTH,DISPLAYLINE_LENGTH,(char*)pMapComm->BtConnectTable[Pointer].PinCode);
+ if (DISPLAYLINE_LENGTH >= 16)
+ {
+ sprintf((char*)TxtBuffer[6],"H=%02X S=%02X Q=%02X",(UWORD)(pMapComm->BtConnectTable[Pointer].HandleNr) & 0xFF,(UWORD)(pMapComm->BtConnectTable[Pointer].StreamStatus) & 0xFF,(UWORD)(pMapComm->BtConnectTable[Pointer].LinkQuality) & 0xFF);
+ }
+ pMapDisplay->EraseMask |= TEXTLINE_BITS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = BT_CONN;
+ }
+ break;
+
+ case BT_CONN :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_ENTER :
+ {
+ State = BT_CONN_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = SYSTEM_INIT;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ if (Pointer)
+ {
+ Pointer--;
+ }
+ else
+ {
+ Pointer = (SIZE_OF_BT_CONNECT_TABLE - 1);
+ }
+ State = BT_CONN_INIT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ if (Pointer < (SIZE_OF_BT_CONNECT_TABLE - 1))
+ {
+ Pointer++;
+ }
+ else
+ {
+ Pointer = 0;
+ }
+ State = BT_CONN_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_INIT :
+ {
+ IOMapUi.Flags &= ~UI_ENABLE_STATUS_UPDATE;
+
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_TESTPRG;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SUBTEST;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+
+ State = TSTPRG_SELECT_SUBTEST;
+ }
+ break;
+
+ case TSTPRG_SELECT_SUBTEST :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ State = TSTPRG_SENSOR_INIT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ State = TSTPRG_MOTOR_INIT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ State = TSTPRG_RCX_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ Count = 0;
+ State = TSTPRG_SKIP;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_SENSOR_INIT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_TESTPRG;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT_SENSOR;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SENSORS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ InputValues[Count] = 0x7FFF;
+ strcpy((char*)TxtBuffer[Count]," ");
+ }
+
+ State = TSTPRG_SELECT_SENSOR;
+ }
+ break;
+
+ case TSTPRG_SELECT_SENSOR :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ State = TSTPRG_TOUCH_SENSOR_INIT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT_TYPE;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SOUND_SENSORS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_SOUND_SENSOR_SELECT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT_TYPE;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_LIGHT_SENSORS;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_LIGHT_SENSOR_SELECT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_TOUCH_SENSOR_INIT :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_SENSOR_TOUCH;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SUBTEST_STOP;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = SWITCH;
+ }
+ State = TSTPRG_TOUCH_SENSOR;
+ }
+ break;
+
+ case TSTPRG_TOUCH_SENSOR :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ if (InputValues[Count] != pMapInput->Inputs[Count].ADRaw)
+ {
+ InputValues[Count] = pMapInput->Inputs[Count].ADRaw;
+ sprintf((char*)TxtBuffer[Count]," Input %u = %4u ",(UWORD)Count + 1,(UWORD)InputValues[Count]);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3 + Count);
+ }
+ }
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ }
+ break;
+
+ case TSTPRG_SOUND_SENSOR_SELECT :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_SENSOR_SOUND_DB;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = SOUND_DB;
+ }
+ State = TSTPRG_SOUND_SENSOR_INIT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_SENSOR_SOUND_DBA;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = SOUND_DBA;
+ }
+ State = TSTPRG_SOUND_SENSOR_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_SOUND_SENSOR_INIT :
+ {
+ VolumeSave = pMapSound->Volume;
+ pMapSound->Volume = MAX_VOLUME;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SOUND_STOP;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_SOUND_SENSOR;
+ }
+ break;
+
+ case TSTPRG_SOUND_SENSOR :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ if (InputValues[Count] != pMapInput->Inputs[Count].ADRaw)
+ {
+ InputValues[Count] = pMapInput->Inputs[Count].ADRaw;
+ sprintf((char*)TxtBuffer[Count]," Input %u = %4u ",(UWORD)Count + 1,(UWORD)InputValues[Count]);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3 + Count);
+ }
+ }
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ pMapSound->Freq = 440;
+ pMapSound->Duration = 2000;
+ pMapSound->Mode = SOUND_TONE;
+ pMapSound->Flags |= SOUND_UPDATE;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ pMapSound->State = SOUND_STOP;
+ pMapSound->Volume = VolumeSave;
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ pMapSound->State = SOUND_STOP;
+ pMapSound->Volume = VolumeSave;
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapSound->Freq = 4000;
+ pMapSound->Duration = 2000;
+ pMapSound->Mode = SOUND_TONE;
+ pMapSound->Flags |= SOUND_UPDATE;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_LIGHT_SENSOR_SELECT :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_SENSOR_LIGHT_PASIVE;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = LIGHT_INACTIVE;
+ }
+ State = TSTPRG_LIGHT_SENSOR_INIT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_SENSOR_LIGHT_ACTIVE;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = LIGHT_ACTIVE;
+ }
+ State = TSTPRG_LIGHT_SENSOR_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_LIGHT_SENSOR_INIT :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SUBTEST_STOP;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_LIGHT_SENSOR;
+ }
+ break;
+
+ case TSTPRG_LIGHT_SENSOR :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ if (InputValues[Count] != pMapInput->Inputs[Count].ADRaw)
+ {
+ InputValues[Count] = pMapInput->Inputs[Count].ADRaw;
+ sprintf((char*)TxtBuffer[Count]," Input %u = %4u ",(UWORD)Count + 1,(UWORD)InputValues[Count]);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3 + Count);
+ }
+ }
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP_SENSOR;
+ }
+ }
+ break;
+
+ case TSTPRG_SKIP_SENSOR :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = NO_SENSOR;
+ }
+ State = TSTPRG_SENSOR_INIT;
+ }
+ break;
+
+ case TSTPRG_MOTOR_INIT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_MOTOR;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_MOTOR_HEADER;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_MOTOR_STOP;
+
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,0);
+ strcpy((char*)TxtBuffer[Count]," ");
+ pMapDisplay->pTextLines[TEXTLINE_4 + Count] = TxtBuffer[Count];
+ }
+
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_MOTOR;
+ }
+ break;
+
+ case TSTPRG_MOTOR :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ if (OutputValues[Count] != (SWORD)TestPrgReadMotor(Count))
+ {
+ OutputValues[Count] = (SWORD)TestPrgReadMotor(Count);
+ sprintf((char*)TxtBuffer[Count]," %c %-4d %4d",(char)Count + 'A',(SWORD)pMapOutPut->Outputs[Count].Speed,OutputValues[Count]);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4 + Count);
+ }
+ }
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,-50);
+ }
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,0);
+ }
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP_MOTOR;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,50);
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_SKIP_MOTOR :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ TestPrgFloatMotor(Count);
+ }
+ State = TSTPRG_INIT;
+ }
+ break;
+
+ case TSTPRG_RCX_INIT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_RCX;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_RCX_STOP;
+
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_RCX_SELECT;
+ }
+ break;
+
+ case TSTPRG_RCX_SELECT :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_SELECT_TYPE;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_RCX_INPUT_SELECT;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ InputValues[Count] = 0x7FFF;
+ strcpy((char*)TxtBuffer[Count]," ");
+ }
+ State = TSTPRG_RCX_INPUT_SELECT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ State = TSTPRG_RCX_DISPLAY_INIT;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ State = TSTPRG_RCX_MOTOR_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_DISPLAY_INIT :
+ {
+ Count = 0;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ State = TSTPRG_RCX_DISPLAY;
+ }
+ break;
+
+ case TSTPRG_RCX_DISPLAY :
+ {
+ if ((Count & 0x7FF) == 0x000)
+ {
+ pMapDisplay->pScreens[SCREEN_BACKGROUND] = (BMPMAP*)Test1;
+ pMapDisplay->UpdateMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ }
+ if ((Count & 0x7FF) == 0x3FF)
+ {
+ pMapDisplay->pScreens[SCREEN_BACKGROUND] = (BMPMAP*)Test2;
+ pMapDisplay->UpdateMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ }
+ Count++;
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP_RCX;
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_INPUT_SELECT :
+ {
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_RCX_INPUT_PASIVE;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = SWITCH;
+ }
+ State = TSTPRG_RCX_INPUT_INIT;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_RCX_INPUT_ACTIVE;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = REFLECTION;
+ }
+ State = TSTPRG_RCX_INPUT_INIT;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP_RCX;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_RCX_INPUT_DIGITAL;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SUBTEST_STOP;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = CUSTOM;
+ }
+ SubState = 0;
+ Timer = 0;
+ State = TSTPRG_RCX_DIGITAL_INIT;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_INPUT_INIT :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3 + Count] = TxtBuffer[Count];
+ }
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_SUBTEST_STOP;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_RCX_INPUT;
+ Timer = 0;
+ }
+ break;
+
+ case TSTPRG_RCX_INPUT :
+ {
+ if (++Timer >= 250)
+ {
+ Timer = 0;
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ if (InputValues[Count] != pMapInput->Inputs[Count].ADRaw)
+ {
+ InputValues[Count] = pMapInput->Inputs[Count].ADRaw;
+ sprintf((char*)TxtBuffer[Count]," Input %u = %4u ",(UWORD)Count + 1,(UWORD)InputValues[Count]);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3 + Count);
+ }
+ }
+ }
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP_RCX;
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_DIGITAL_INIT :
+ {
+ if (++Timer >= 20)
+ {
+ Timer = 0;
+ switch (SubState)
+ {
+ case 0 :
+ {
+ pMapInput->Inputs[0].DigiPinsDir |= DIGI0; // Digi 1-0 output -,
+ pMapInput->Inputs[0].DigiPinsDir &= ~DIGI1; // Digi 1-1 input -, |
+ pMapInput->Inputs[2].DigiPinsDir &= ~DIGI0; // Digi 3-0 input | -'
+ pMapInput->Inputs[2].DigiPinsDir |= DIGI1; // Digi 3-1 output -'
+
+ pMapInput->Inputs[0].DigiPinsOut |= DIGI0; // Digi 1-0 output high
+ pMapInput->Inputs[2].DigiPinsOut &= ~DIGI1; // Digi 3-1 output low
+
+ SubState++;
+ }
+ break;
+
+ case 1 :
+ {
+ if ((pMapInput->Inputs[2].DigiPinsIn & DIGI0) && !(pMapInput->Inputs[0].DigiPinsIn & DIGI1))
+ {
+ pMapInput->Inputs[0].DigiPinsOut &= ~DIGI0; // Digi 1-0 output low
+ pMapInput->Inputs[2].DigiPinsOut |= DIGI1; // Digi 3-1 output high
+
+ SubState++;
+ }
+ else
+ {
+ State = TSTPRG_RCX_DIGITAL_FAIL;
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ if (!(pMapInput->Inputs[2].DigiPinsIn & DIGI0) && (pMapInput->Inputs[0].DigiPinsIn & DIGI1))
+ {
+ pMapInput->Inputs[0].DigiPinsDir &= ~DIGI0; // Digi 1-0 input
+ pMapInput->Inputs[0].DigiPinsDir &= ~DIGI1; // Digi 1-1 input
+ pMapInput->Inputs[2].DigiPinsDir &= ~DIGI0; // Digi 3-0 input
+ pMapInput->Inputs[2].DigiPinsDir &= ~DIGI1; // Digi 3-1 input
+
+ pMapInput->Inputs[1].DigiPinsDir |= DIGI0; // Digi 2-0 output -,
+ pMapInput->Inputs[1].DigiPinsDir &= ~DIGI1; // Digi 2-1 input -, |
+ pMapInput->Inputs[3].DigiPinsDir &= ~DIGI0; // Digi 4-0 input | -'
+ pMapInput->Inputs[3].DigiPinsDir |= DIGI1; // Digi 4-1 output -'
+
+ pMapInput->Inputs[1].DigiPinsOut |= DIGI0; // Digi 2-0 output high
+ pMapInput->Inputs[3].DigiPinsOut &= ~DIGI1; // Digi 4-1 output low
+
+ SubState++;
+ }
+ else
+ {
+ State = TSTPRG_RCX_DIGITAL_FAIL;
+ }
+ }
+ break;
+
+ case 3 :
+ {
+ if ((pMapInput->Inputs[3].DigiPinsIn & DIGI0) && !(pMapInput->Inputs[1].DigiPinsIn & DIGI1))
+ {
+ pMapInput->Inputs[1].DigiPinsOut &= ~DIGI0; // Digi 2-0 output low
+ pMapInput->Inputs[3].DigiPinsOut |= DIGI1; // Digi 4-1 output high
+
+ SubState++;
+ }
+ else
+ {
+ State = TSTPRG_RCX_DIGITAL_FAIL;
+ }
+ }
+ break;
+
+ case 4 :
+ {
+ if (!(pMapInput->Inputs[3].DigiPinsIn & DIGI0) && (pMapInput->Inputs[1].DigiPinsIn & DIGI1))
+ {
+ pMapInput->Inputs[1].DigiPinsDir &= ~DIGI0; // Digi 2-0 input
+ pMapInput->Inputs[1].DigiPinsDir &= ~DIGI1; // Digi 2-1 input
+ pMapInput->Inputs[3].DigiPinsDir &= ~DIGI0; // Digi 4-0 input
+ pMapInput->Inputs[3].DigiPinsDir &= ~DIGI1; // Digi 4-1 input
+
+ State = TSTPRG_RCX_DIGITAL_OK;
+ }
+ else
+ {
+ State = TSTPRG_RCX_DIGITAL_FAIL;
+ }
+ }
+ break;
+
+ }
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_DIGITAL_OK :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_RCX_DIGITAL_OK;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_RCX_DIGITAL;
+ }
+ break;
+
+ case TSTPRG_RCX_DIGITAL_FAIL :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_RCX_DIGITAL_FAIL;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_RCX_DIGITAL;
+ }
+ break;
+
+ case TSTPRG_RCX_DIGITAL :
+ {
+ if (cUiReadButtons() != BUTTON_NONE)
+ {
+ State = TSTPRG_SKIP_RCX;
+ }
+ }
+ break;
+
+ case TSTPRG_RCX_MOTOR_INIT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_1] = (UBYTE*)TXT_MOTOR;
+ pMapDisplay->pTextLines[TEXTLINE_2] = (UBYTE*)TXT_LINE;
+ pMapDisplay->pTextLines[TEXTLINE_3] = (UBYTE*)TXT_MOTOR_HEADER;
+ pMapDisplay->pTextLines[TEXTLINE_4] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_5] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_6] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_7] = (UBYTE*)TXT_EMPTY;
+ pMapDisplay->pTextLines[TEXTLINE_8] = (UBYTE*)TXT_MOTOR_NEXT;
+
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,0);
+ strcpy((char*)TxtBuffer[Count]," ");
+ pMapDisplay->pTextLines[TEXTLINE_4 + Count] = TxtBuffer[Count];
+ }
+
+ Pointer = 0;
+ pMapDisplay->UpdateMask |= TEXTLINE_BITS;
+ State = TSTPRG_RCX_MOTOR;
+ }
+ break;
+
+ case TSTPRG_RCX_MOTOR :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ if (OutputValues[Count] != (SWORD)TestPrgReadMotor(Count))
+ {
+ OutputValues[Count] = (SWORD)TestPrgReadMotor(Count);
+ if (Pointer == Count)
+ {
+ sprintf((char*)TxtBuffer[Count],"> %c %-4d %4d",(char)Count + 'A',(SWORD)pMapOutPut->Outputs[Count].Speed,OutputValues[Count]);
+ }
+ else
+ {
+ sprintf((char*)TxtBuffer[Count]," %c %-4d %4d",(char)Count + 'A',(SWORD)pMapOutPut->Outputs[Count].Speed,OutputValues[Count]);
+ }
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4 + Count);
+ }
+ }
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ if (Pointer == Count)
+ {
+ TestPrgRunMotor(Count,-50);
+ }
+ else
+ {
+ TestPrgRunMotor(Count,0);
+ }
+ }
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ TestPrgRunMotor(Count,0);
+ }
+ if (++Pointer >= NO_OF_OUTPUTS)
+ {
+ State = TSTPRG_SKIP_RCX_MOTOR;
+ }
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ State = TSTPRG_SKIP_RCX_MOTOR;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ OutputValues[Count] = 0x7FFF;
+ if (Pointer == Count)
+ {
+ TestPrgRunMotor(Count,50);
+ }
+ else
+ {
+ TestPrgRunMotor(Count,0);
+ }
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case TSTPRG_SKIP_RCX_MOTOR :
+ {
+ for (Count = 0;Count < NO_OF_OUTPUTS;Count++)
+ {
+ TestPrgFloatMotor(Count);
+ }
+ State = TSTPRG_RCX_INIT;
+ }
+ break;
+
+ case TSTPRG_SKIP_RCX :
+ {
+ for (Count = 0;Count < NO_OF_INPUTS;Count++)
+ {
+ pMapInput->Inputs[Count].SensorType = NO_SENSOR;
+ }
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ State = TSTPRG_RCX_INIT;
+ }
+ break;
+
+#endif
+
+ case TSTPRG_SKIP :
+ {
+ Count++;
+ if (Count == 100)
+ {
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ }
+ if (Count >= 200)
+ {
+ IOMapUi.Flags |= UI_REDRAW_STATUS;
+ State = 0;
+ }
+ }
+ break;
+
+ default :
+ {
+ State = SYSTEM_INIT;
+ }
+ break;
+
+ }
+
+ return (State);
+}
diff --git a/AT91SAM7S256/Source/Connections.txt b/AT91SAM7S256/Source/Connections.txt
new file mode 100644
index 0000000..3c20ed0
--- /dev/null
+++ b/AT91SAM7S256/Source/Connections.txt
@@ -0,0 +1,23 @@
+DEFINE_DATA(ICON, Connections) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x01,0x20, // Graphics DataSize
+ 0x01, // Graphics Count X
+ 0x04, // Graphics Count Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDB,0x00,0x00,0x7E,0x81,0x81,0x7E,0x00,0x00,0xDB,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDB,0x00,0x00,0x00,0x82,0xFF,0x80,0x00,0x00,0xDB,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDB,0x00,0x00,0xE2,0x91,0x89,0x86,0x00,0x00,0xDB,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x40,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDB,0x00,0x00,0x42,0x81,0x89,0x76,0x00,0x00,0xDB,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Cursor.txt b/AT91SAM7S256/Source/Cursor.txt
new file mode 100644
index 0000000..69037cd
--- /dev/null
+++ b/AT91SAM7S256/Source/Cursor.txt
@@ -0,0 +1,13 @@
+#define sizeof_Cursor 15
+DEFINE_DATA(BMPMAP, Cursor) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x08, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x07, // Graphics Width
+ 0x08, // Graphics Height
+BEGIN_DATA
+ 0x21,0x31,0x39,0x3D,0x39,0x31,0x21
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Devices.txt b/AT91SAM7S256/Source/Devices.txt
new file mode 100644
index 0000000..cbfd564
--- /dev/null
+++ b/AT91SAM7S256/Source/Devices.txt
@@ -0,0 +1,23 @@
+DEFINE_DATA(ICON, Devices) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x01,0x20, // Graphics DataSize
+ 0x01, // Graphics Count X
+ 0x04, // Graphics Count Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x82,0x44,0x28,0xFF,0x11,0xAA,0x44,0x00,0x00,0x06,0x01,0x00,0x40,0x20,0x11,0x0E,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x24,0x3C,0x25,0x25,0x25,0x3C,0x24,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x70,0x08,0x30,0x40,0x40,0x40,0x40,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x4E,0x91,0x51,0x91,0x51,0x8E,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0F,0x10,0x11,0x12,0x11,0x12,0x11,0x12,0x10,0x0F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xBF,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x06,0x07,0x05,0x06,0x05,0x06,0x05,0x06,0x05,0x06,0x05,0x06,0x05,0x07,0x06,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Display.txt b/AT91SAM7S256/Source/Display.txt
new file mode 100644
index 0000000..d1c2136
--- /dev/null
+++ b/AT91SAM7S256/Source/Display.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, Display) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xD8, // Graphics DataSize
+ 0x0E, // Graphics Start X
+ 0x10, // Graphics Start Y
+ 0x48, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0xF8,0xFC,0x0E,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x0E,0xFC,0xF8,0xC0,
+ 0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,
+ 0x0F,0x1F,0x38,0x30,0x30,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x78,0x7F,0x3F,0x1F
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Fail.txt b/AT91SAM7S256/Source/Fail.txt
new file mode 100644
index 0000000..a213ec5
--- /dev/null
+++ b/AT91SAM7S256/Source/Fail.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, Fail) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x48, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x08, // Graphics Start Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x60,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x30,0x0C,0x03,0x00,0x7C,0x00,0x03,0x0C,0x30,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x04,0x04,0x04,0x04,0x05,0x04,0x04,0x04,0x04,0x04,0x03,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Font.txt b/AT91SAM7S256/Source/Font.txt
new file mode 100644
index 0000000..0aa4303
--- /dev/null
+++ b/AT91SAM7S256/Source/Font.txt
@@ -0,0 +1,17 @@
+DEFINE_DATA(ICON, Font) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x02,0x40, // Graphics DataSize
+ 0x10, // Graphics Count X
+ 0x06, // Graphics Count Y
+ 0x06, // Graphics Width
+ 0x08, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x06,0x5F,0x06,0x00,0x00,0x07,0x03,0x00,0x07,0x03,0x00,0x24,0x7E,0x24,0x7E,0x24,0x00,0x24,0x2B,0x6A,0x12,0x00,0x00,0x63,0x13,0x08,0x64,0x63,0x00,0x30,0x4C,0x52,0x22,0x50,0x00,0x00,0x07,0x03,0x00,0x00,0x00,0x00,0x3E,0x41,0x00,0x00,0x00,0x00,0x41,0x3E,0x00,0x00,0x00,0x08,0x3E,0x1C,0x3E,0x08,0x00,0x08,0x08,0x3E,0x08,0x08,0x00,0x80,0x60,0x60,0x00,0x00,0x00,0x08,0x08,0x08,0x08,0x08,0x00,0x00,0x60,0x60,0x00,0x00,0x00,0x20,0x10,0x08,0x04,0x02,0x00,
+ 0x3E,0x51,0x49,0x45,0x3E,0x00,0x00,0x42,0x7F,0x40,0x00,0x00,0x62,0x51,0x49,0x49,0x46,0x00,0x22,0x49,0x49,0x49,0x36,0x00,0x18,0x14,0x12,0x7F,0x10,0x00,0x2F,0x49,0x49,0x49,0x31,0x00,0x3C,0x4A,0x49,0x49,0x30,0x00,0x01,0x71,0x09,0x05,0x03,0x00,0x36,0x49,0x49,0x49,0x36,0x00,0x06,0x49,0x49,0x29,0x1E,0x00,0x00,0x6C,0x6C,0x00,0x00,0x00,0x00,0xEC,0x6C,0x00,0x00,0x00,0x08,0x14,0x22,0x41,0x00,0x00,0x24,0x24,0x24,0x24,0x24,0x00,0x00,0x41,0x22,0x14,0x08,0x00,0x02,0x01,0x59,0x09,0x06,0x00,
+ 0x3E,0x41,0x5D,0x55,0x1E,0x00,0x7E,0x11,0x11,0x11,0x7E,0x00,0x7F,0x49,0x49,0x49,0x36,0x00,0x3E,0x41,0x41,0x41,0x22,0x00,0x7F,0x41,0x41,0x41,0x3E,0x00,0x7F,0x49,0x49,0x49,0x41,0x00,0x7F,0x09,0x09,0x09,0x01,0x00,0x3E,0x41,0x49,0x49,0x7A,0x00,0x7F,0x08,0x08,0x08,0x7F,0x00,0x00,0x41,0x7F,0x41,0x00,0x00,0x30,0x40,0x40,0x40,0x3F,0x00,0x7F,0x08,0x14,0x22,0x41,0x00,0x7F,0x40,0x40,0x40,0x40,0x00,0x7F,0x02,0x04,0x02,0x7F,0x00,0x7F,0x02,0x04,0x08,0x7F,0x00,0x3E,0x41,0x41,0x41,0x3E,0x00,
+ 0x7F,0x09,0x09,0x09,0x06,0x00,0x3E,0x41,0x51,0x21,0x5E,0x00,0x7F,0x09,0x09,0x19,0x66,0x00,0x26,0x49,0x49,0x49,0x32,0x00,0x01,0x01,0x7F,0x01,0x01,0x00,0x3F,0x40,0x40,0x40,0x3F,0x00,0x1F,0x20,0x40,0x20,0x1F,0x00,0x3F,0x40,0x3C,0x40,0x3F,0x00,0x63,0x14,0x08,0x14,0x63,0x00,0x07,0x08,0x70,0x08,0x07,0x00,0x71,0x49,0x45,0x43,0x00,0x00,0x00,0x7F,0x41,0x41,0x00,0x00,0x02,0x04,0x08,0x10,0x20,0x00,0x00,0x41,0x41,0x7F,0x00,0x00,0x04,0x02,0x01,0x02,0x04,0x00,0x80,0x80,0x80,0x80,0x80,0x00,
+ 0x00,0x02,0x05,0x02,0x00,0x00,0x20,0x54,0x54,0x54,0x78,0x00,0x7F,0x44,0x44,0x44,0x38,0x00,0x38,0x44,0x44,0x44,0x28,0x00,0x38,0x44,0x44,0x44,0x7F,0x00,0x38,0x54,0x54,0x54,0x08,0x00,0x08,0x7E,0x09,0x09,0x00,0x00,0x18,0x24,0xA4,0xA4,0xFC,0x00,0x7F,0x04,0x04,0x78,0x00,0x00,0x00,0x00,0x7D,0x40,0x00,0x00,0x40,0x80,0x84,0x7D,0x00,0x00,0x7F,0x10,0x28,0x44,0x00,0x00,0x00,0x00,0x7F,0x40,0x00,0x00,0x7C,0x04,0x18,0x04,0x78,0x00,0x7C,0x04,0x04,0x78,0x00,0x00,0x38,0x44,0x44,0x44,0x38,0x00,
+ 0xFC,0x44,0x44,0x44,0x38,0x00,0x38,0x44,0x44,0x44,0xFC,0x00,0x44,0x78,0x44,0x04,0x08,0x00,0x08,0x54,0x54,0x54,0x20,0x00,0x04,0x3E,0x44,0x24,0x00,0x00,0x3C,0x40,0x20,0x7C,0x00,0x00,0x1C,0x20,0x40,0x20,0x1C,0x00,0x3C,0x60,0x30,0x60,0x3C,0x00,0x6C,0x10,0x10,0x6C,0x00,0x00,0x9C,0xA0,0x60,0x3C,0x00,0x00,0x64,0x54,0x54,0x4C,0x00,0x00,0x08,0x3E,0x41,0x41,0x00,0x00,0x00,0x00,0x77,0x00,0x00,0x00,0x00,0x41,0x41,0x3E,0x08,0x00,0x02,0x01,0x02,0x01,0x00,0x00,0x10,0x20,0x40,0x38,0x07,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Functions.inl b/AT91SAM7S256/Source/Functions.inl
new file mode 100644
index 0000000..1ef019e
--- /dev/null
+++ b/AT91SAM7S256/Source/Functions.inl
@@ -0,0 +1,4331 @@
+//
+// Programmer
+//
+// Date init 26.04.2005
+//
+// Reviser $Author:: Dktochpe $
+//
+// Revision date $Date:: $
+//
+// Filename $Workfile:: $
+//
+// Version $Revision:: $
+//
+// Archive $Archive:: $
+//
+// Platform C
+//
+
+//******* cUiBtTest **********************************************************
+
+const UBYTE NONVOLATILE_NAME[] = UI_NONVOLATILE; // Non volatile filename without extention
+const UBYTE DEFAULT_PROGRAM_NAME[] = UI_PROGRAM_DEFAULT; // On brick programming filename without extention
+const UBYTE TEMP_PROGRAM_FILENAME[] = UI_PROGRAM_TEMP; // On brick programming tmp filename without extention
+const UBYTE VM_PROGRAM_READER[] = UI_PROGRAM_READER; // On brick programming script reader filename without extention
+const UBYTE TEMP_DATALOG_FILENAME[] = UI_DATALOG_TEMP; // On brick datalog tmp filename without extention
+const UBYTE DEFAULT_DATALOG_NAME[] = UI_DATALOG_DEFAULT; // On brick datalog filename without extention
+const UBYTE DEFAULT_PIN_CODE[] = UI_PINCODE_DEFAULT; // Default blue tooth pin code
+const UBYTE TXT_INVALID_SENSOR[] = "??????????????"; // Display invalid sensor data
+
+
+#define SENSORS (MENU_SENSOR_INVALID - MENU_SENSOR_EMPTY)
+
+const UBYTE SENSORTYPE[SENSORS] = // for view and datalog
+{
+ 0, // MENU_SENSOR_EMPTY
+ SOUND_DB, // MENU_SENSOR_SOUND_DB
+ SOUND_DBA, // MENU_SENSOR_SOUND_DBA
+ LIGHT_ACTIVE, // MENU_SENSOR_LIGHT
+ LIGHT_INACTIVE, // MENU_SENSOR_LIGHT_AMB
+ SWITCH, // MENU_SENSOR_TOUCH
+ 0, // MENU_SENSOR_MOTOR_DEG
+ 0, // MENU_SENSOR_MOTOR_ROT
+ LOWSPEED_9V, // MENU_SENSOR_ULTRASONIC_IN
+ LOWSPEED_9V, // MENU_SENSOR_ULTRASONIC_CM
+ LOWSPEED_9V, // MENU_SENSOR_IIC_TEMP_C
+ LOWSPEED_9V, // MENU_SENSOR_IIC_TEMP_F
+ COLORFULL // MENU_SENSOR_COLOR
+};
+
+const UBYTE SENSORMODE[SENSORS] = // for view and datalog
+{
+ 0, // MENU_SENSOR_EMPTY
+ PCTFULLSCALEMODE, // MENU_SENSOR_SOUND_DB
+ PCTFULLSCALEMODE, // MENU_SENSOR_SOUND_DBA
+ PCTFULLSCALEMODE, // MENU_SENSOR_LIGHT
+ PCTFULLSCALEMODE, // MENU_SENSOR_LIGHT_AMB
+ BOOLEANMODE, // MENU_SENSOR_TOUCH
+ 0, // MENU_SENSOR_MOTOR_DEG
+ 0, // MENU_SENSOR_MOTOR_ROT
+ 0, // MENU_SENSOR_ULTRASONIC_IN
+ 0, // MENU_SENSOR_ULTRASONIC_CM
+ 0, // MENU_SENSOR_IIC_TEMP_C
+ 0, // MENU_SENSOR_IIC_TEMP_F
+ 0 // MENU_SENSOR_COLOR
+};
+
+const UBYTE SENSORFORMAT[SENSORS][9] =
+{
+ "", // MENU_SENSOR_EMPTY
+ "%3.0f %%", // MENU_SENSOR_SOUND_DB
+ "%3.0f %%", // MENU_SENSOR_SOUND_DBA
+ "%3.0f %%", // MENU_SENSOR_LIGHT
+ "%3.0f %%", // MENU_SENSOR_LIGHT_AMB
+ "%1.0f", // MENU_SENSOR_TOUCH
+ "%8.0f `", // MENU_SENSOR_MOTOR_DEG
+ "%8.0f R", // MENU_SENSOR_MOTOR_ROT
+ "%3.0f In", // MENU_SENSOR_ULTRASONIC_IN
+ "%3.0f cm", // MENU_SENSOR_ULTRASONIC_CM
+ "%5.1f `C", // MENU_SENSOR_IIC_TEMP_C
+ "%5.1f `F", // MENU_SENSOR_IIC_TEMP_F
+ "%9.0f" // MENU_SENSOR_COLOR (no of characters)
+};
+
+const float SENSORDIVIDER[SENSORS] =
+{
+ 1.0, // MENU_SENSOR_EMPTY
+ 1.0, // MENU_SENSOR_SOUND_DB
+ 1.0, // MENU_SENSOR_SOUND_DBA
+ 1.0, // MENU_SENSOR_LIGHT
+ 1.0, // MENU_SENSOR_LIGHT_AMB
+ 1.0, // MENU_SENSOR_TOUCH
+ 1.0, // MENU_SENSOR_MOTOR_DEG
+ 360.0, // MENU_SENSOR_MOTOR_ROT
+ 2.54, // MENU_SENSOR_ULTRASONIC_IN
+ 1.0, // MENU_SENSOR_ULTRASONIC_CM
+ 10.0, // MENU_SENSOR_IIC_TEMP_C
+ 10.0, // MENU_SENSOR_IIC_TEMP_F
+ 1.0 // MENU_SENSOR_COLOR
+};
+
+
+#define SENSORSYNCDATA "Sync data"
+#define SENSORSDATA "Sdata"
+#define SENSORTIME "Time"
+
+
+const UBYTE SENSORDIRNAME[SENSORS - 1][19] =
+{
+ "Sound Sensor", // MENU_SENSOR_SOUND_DB
+ "Sound Sensor", // MENU_SENSOR_SOUND_DBA
+ "Light Sensor", // MENU_SENSOR_LIGHT
+ "Light Sensor", // MENU_SENSOR_LIGHT_AMB
+ "Bumper", // MENU_SENSOR_TOUCH
+ "FP Rotation Sensor", // MENU_SENSOR_MOTOR_DEG
+ "FP Rotation Sensor", // MENU_SENSOR_MOTOR_ROT
+ "Distance Sensor", // MENU_SENSOR_ULTRASONIC_IN
+ "Distance Sensor", // MENU_SENSOR_ULTRASONIC_CM
+ "NXT Temp Sensor", // MENU_SENSOR_IIC_TEMP_C
+ "NXT Temp Sensor", // MENU_SENSOR_IIC_TEMP_F
+ "Color Detector" // MENU_SENSOR_COLOR
+};
+
+const UBYTE SENSORUNITNAME[SENSORS - 1][5] =
+{
+ "_dB", // MENU_SENSOR_SOUND_DB
+ "_dBa", // MENU_SENSOR_SOUND_DBA
+ "_on", // MENU_SENSOR_LIGHT
+ "_off", // MENU_SENSOR_LIGHT_AMB
+ "", // MENU_SENSOR_TOUCH
+ "_deg", // MENU_SENSOR_MOTOR_DEG
+ "_rot", // MENU_SENSOR_MOTOR_ROT
+ "_in", // MENU_SENSOR_ULTRASONIC_IN
+ "_cm", // MENU_SENSOR_ULTRASONIC_CM
+ "_C", // MENU_SENSOR_IIC_TEMP_C
+ "_F", // MENU_SENSOR_IIC_TEMP_F
+ "_0", // MENU_SENSOR_COLOR
+};
+
+const UBYTE SENSORFORMAT2[SENSORS - 1][6] =
+{
+ "\t%.0f", // MENU_SENSOR_SOUND_DB
+ "\t%.0f", // MENU_SENSOR_SOUND_DBA
+ "\t%.0f", // MENU_SENSOR_LIGHT
+ "\t%.0f", // MENU_SENSOR_LIGHT_AMB
+ "\t%.0f", // MENU_SENSOR_TOUCH
+ "\t%.0f", // MENU_SENSOR_MOTOR_DEG
+ "\t%.0f", // MENU_SENSOR_MOTOR_ROT
+ "\t%.0f", // MENU_SENSOR_ULTRASONIC_IN
+ "\t%.0f", // MENU_SENSOR_ULTRASONIC_CM
+ "\t%.1f", // MENU_SENSOR_IIC_TEMP_C
+ "\t%.1f", // MENU_SENSOR_IIC_TEMP_F
+ "\t%.0f" // MENU_SENSOR_COLOR
+};
+
+
+//******* cUiWriteLowspeed ***************************************************
+
+void cUiWriteLowspeed(UBYTE Port,UBYTE TxBytes,UBYTE *TxBuf,UBYTE RxBytes)
+{
+ Port -= MENU_PORT_1;
+ pMapLowSpeed->OutBuf[Port].InPtr = 0;
+ pMapLowSpeed->OutBuf[Port].OutPtr = 0;
+
+ while (TxBytes)
+ {
+ pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = *TxBuf;
+ pMapLowSpeed->OutBuf[Port].InPtr++;
+ TxBuf++;
+ TxBytes--;
+ }
+ pMapLowSpeed->InBuf[Port].BytesToRx = RxBytes;
+ pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT;
+ pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port);
+}
+
+
+//******* cUiReadLowspeed ****************************************************
+
+#define IIC_READY 0
+#define IIC_BUSY 1
+#define IIC_ERROR 2
+
+UBYTE cUiReadLowspeed(UBYTE Port,UBYTE RxBytes,UWORD *Value)
+{
+ UBYTE Result;
+
+ *Value = 0;
+ Port -= MENU_PORT_1;
+ if ((pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_DONE))
+ {
+ while (RxBytes)
+ {
+ (*Value) <<= 8;
+ (*Value) |= (UWORD)(pMapLowSpeed->InBuf[Port].Buf[pMapLowSpeed->InBuf[Port].OutPtr]);
+ pMapLowSpeed->InBuf[Port].OutPtr++;
+ if (pMapLowSpeed->InBuf[Port].OutPtr >= SIZE_OF_LSBUF)
+ {
+ pMapLowSpeed->InBuf[Port].OutPtr = 0;
+ }
+ RxBytes--;
+ }
+ Result = IIC_READY;
+ }
+ else
+ {
+ if (pMapLowSpeed->ErrorType[Port] == LOWSPEED_CH_NOT_READY)
+ {
+ Result = IIC_ERROR;
+ }
+ else
+ {
+ Result = IIC_BUSY;
+ }
+ }
+
+ return (Result);
+}
+
+
+//******* cUiUpdateSensor ****************************************************
+
+#define SENSOR_SETUP 0
+#define SENSOR_ACQUIRE 3
+#define SENSOR_READ 8
+#define SENSOR_STATES 10
+
+void cUiUpdateSensor(SWORD Time)
+{
+ UBYTE Port;
+ UBYTE Sensor;
+ UBYTE Result;
+ SWORD Tmp;
+
+ if (VarsUi.SensorReset == TRUE)
+ {
+ for (Port = MENU_PORT_1;Port < MENU_PORT_INVALID;Port++)
+ {
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = FALSE;
+ }
+ VarsUi.SensorTimer = (MIN_SENSOR_READ_TIME / SENSOR_STATES);
+ VarsUi.SensorState = SENSOR_SETUP;
+ }
+
+ VarsUi.SensorTimer += Time;
+ if (VarsUi.SensorTimer >= (MIN_SENSOR_READ_TIME / SENSOR_STATES))
+ {
+ VarsUi.SensorTimer -= (MIN_SENSOR_READ_TIME / SENSOR_STATES);
+
+ for (Port = MENU_PORT_1;Port < MENU_PORT_INVALID;Port++)
+ {
+ Sensor = VarsUi.DatalogPort[Port - MENU_PORT_1];
+
+ if (Sensor != MENU_SENSOR_EMPTY)
+ {
+ if ((Sensor == MENU_SENSOR_MOTOR_DEG) || (Sensor == MENU_SENSOR_MOTOR_ROT))
+ {
+ if (VarsUi.SensorReset == TRUE)
+ {
+ pMapOutPut->Outputs[Port - MENU_PORT_A].Mode &= ~(BRAKE | MOTORON);
+ pMapOutPut->Outputs[Port - MENU_PORT_A].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_RESET_COUNT;
+ pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt = 0;
+ }
+ if (VarsUi.SensorState == SENSOR_READ)
+ {
+ VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt;
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = TRUE;
+ }
+ }
+ else
+ {
+ pMapInput->Inputs[Port - MENU_PORT_1].SensorType = SENSORTYPE[Sensor - MENU_SENSOR_EMPTY];
+ pMapInput->Inputs[Port - MENU_PORT_1].SensorMode = SENSORMODE[Sensor - MENU_SENSOR_EMPTY];
+ if ((Sensor == MENU_SENSOR_ULTRASONIC_IN) || (Sensor == MENU_SENSOR_ULTRASONIC_CM))
+ {
+ if (VarsUi.SensorReset == TRUE)
+ {
+ cUiWriteLowspeed(Port,3,"\x02\x41\x02",0);
+ }
+ if (VarsUi.SensorState == SENSOR_ACQUIRE)
+ {
+ cUiWriteLowspeed(Port,2,"\x02\x42",1);
+ }
+ if (VarsUi.SensorState == SENSOR_READ)
+ {
+ Result = cUiReadLowspeed(Port,1,(UWORD*)&Tmp);
+ if (Result == IIC_READY)
+ {
+ if ((UBYTE)Tmp != 0xFF)
+ {
+ VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)Tmp;
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = TRUE;
+ }
+ else
+ {
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = FALSE;
+ }
+ }
+ else
+ {
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = FALSE;
+ }
+ }
+ }
+ else
+ {
+ if ((Sensor == MENU_SENSOR_IIC_TEMP_C) || (Sensor == MENU_SENSOR_IIC_TEMP_F))
+ {
+ if (VarsUi.SensorState == SENSOR_SETUP)
+ {
+ cUiWriteLowspeed(Port,3,"\x98\x01\x60",0);
+ }
+ if (VarsUi.SensorState == SENSOR_ACQUIRE)
+ {
+ cUiWriteLowspeed(Port,2,"\x98\x00",2);
+ }
+ if (VarsUi.SensorState == SENSOR_READ)
+ {
+ Result = cUiReadLowspeed(Port,2,(UWORD*)&Tmp);
+ if (Result == IIC_READY)
+ {
+// if (Tmp >= -14080)
+ {
+ if (Sensor == MENU_SENSOR_IIC_TEMP_F)
+ {
+ VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)(Tmp + 4544) / 14.2);
+ }
+ else
+ {
+ VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)Tmp / 25.6);
+ }
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = TRUE;
+ }
+ }
+ else
+ {
+ if (Result == IIC_ERROR)
+ {
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = FALSE;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (VarsUi.SensorState == SENSOR_READ)
+ {
+ if (pMapInput->Inputs[Port - MENU_PORT_1].InvalidData != INVALID_DATA)
+ {
+ VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = pMapInput->Inputs[Port - MENU_PORT_1].SensorValue;
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = TRUE;
+ }
+ else
+ {
+ VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = FALSE;
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ VarsUi.SensorState++;
+ if (VarsUi.SensorState >= SENSOR_STATES)
+ {
+ VarsUi.SensorState = SENSOR_SETUP;
+ }
+
+ VarsUi.SensorReset = FALSE;
+ }
+}
+
+
+//******* cUiGetCustomPctFullScale *******************************************
+
+
+UBYTE cUiGetCustomPctFullScale(UBYTE Port,UBYTE Sensor)
+{
+ UBYTE Result = 0;
+
+ if ((Sensor != MENU_SENSOR_MOTOR_DEG) && (Sensor != MENU_SENSOR_MOTOR_ROT))
+ {
+ Result = (UBYTE)pMapInput->Inputs[Port - MENU_PORT_1].CustomPctFullScale;
+ }
+
+ return (Result);
+}
+
+
+
+//******* cUiGetCustomActiveStatus *******************************************
+
+
+UBYTE cUiGetCustomActiveStatus(UBYTE Port,UBYTE Sensor)
+{
+ UBYTE Result = 0;
+
+ if ((Sensor != MENU_SENSOR_MOTOR_DEG) && (Sensor != MENU_SENSOR_MOTOR_ROT))
+ {
+ Result = (UBYTE)pMapInput->Inputs[Port - MENU_PORT_1].CustomActiveStatus;
+ }
+
+ return (Result);
+}
+
+
+
+//******* cUiPrintSensorInDisplayBuffer **************************************
+
+#define COLORNAMES 6
+
+const UBYTE COLORNAME[COLORNAMES][10] =
+{
+ "1. Black ",
+ "2. Blue ",
+ "3. Green ",
+ "4. Yellow",
+ "5. Red ",
+ "6. White "
+};
+
+
+void cUiPrintSensorInDisplayBuffer(UBYTE Port)
+{
+ UBYTE Sensor;
+ float Value;
+ SWORD Size;
+ SWORD Index;
+
+ Port -= MENU_PORT_1;
+ Sensor = VarsUi.DatalogPort[Port] - MENU_SENSOR_EMPTY;
+ Value = (float)VarsUi.DatalogSampleValue[Port] / (float)SENSORDIVIDER[Sensor];
+ Size = sprintf((char*)VarsUi.DisplayBuffer,(char*)SENSORFORMAT[Sensor],(float)0);
+ sprintf((char*)VarsUi.DisplayBuffer,"%*.*s",Size,Size,(char*)TXT_INVALID_SENSOR);
+
+ if (VarsUi.DatalogSampleValid[Port] == TRUE)
+ {
+ if (Sensor == (MENU_SENSOR_COLOR - MENU_SENSOR_EMPTY))
+ {
+ Index = (SWORD)Value - 1;
+ if ((Index >= 0) && (Index < COLORNAMES))
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,(char*)COLORNAME[Index]);
+ }
+ }
+ else
+ {
+ if (Size < sprintf((char*)VarsUi.DisplayBuffer,(char*)SENSORFORMAT[Sensor],Value))
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,"%*.*s",Size,Size,(char*)TXT_INVALID_SENSOR);
+ }
+ }
+ }
+}
+
+
+//******* cUiReleaseSensors **************************************************
+
+void cUiReleaseSensors(void)
+{
+ UBYTE Tmp;
+
+ for (Tmp = 0;Tmp < NO_OF_INPUTS;Tmp++)
+ {
+ pMapInput->Inputs[Tmp].SensorType = NO_SENSOR;
+ }
+}
+
+
+
+//******* cUiBtCommand *******************************************************
+
+enum
+{
+ UI_BT_CTRL,
+
+ UI_BT_GET_DEVICES, // (UI_BT_GET_DEVICES,Known,*pDevices,NULL) [Known = 0,1]
+ UI_BT_GET_DEVICE_NAME, // (UI_BT_GET_DEVICE_NAME,Known,*pIndex,*pDeviceName) [Known = 0,1]
+ UI_BT_GET_DEVICE_TYPE, // (UI_BT_GET_DEVICE_TYPE,Known,*pIndex,*pDeviceType) [Known = 0,1]
+
+ UI_BT_GET_CONNECTION_NAME, // (UI_BT_GET_CONNECTION_NAME,NULL,*pConnection,*pConnectionName)
+ UI_BT_GET_CONNECTION_TYPE, // (UI_BT_GET_CONNECTION_TYPE,NULL,*pConnection,*pConnectionType)
+ UI_BT_GET_CONNECTION_VALID, // (UI_BT_GET_CONNECTION_NAME,NULL,*pConnection,NULL)
+
+ UI_BT_DUMMY
+};
+
+
+#define UI_BT_FAILED 0x8200 // General command failed
+#define UI_BT_SUCCES 0x0000 // Command executed succesfully
+
+
+UBYTE cUiBTGetDeviceType(UBYTE *pCOD)
+{
+ ULONG COD;
+ UBYTE Result;
+ UBYTE Tmp;
+
+ COD = 0;
+ for (Tmp = 0;Tmp < SIZE_OF_CLASS_OF_DEVICE;Tmp++)
+ {
+ COD <<= 8;
+ COD |= (ULONG)*pCOD;
+ pCOD++;
+ }
+
+ Result = DEVICETYPE_UNKNOWN;
+ if ((COD & 0x00001FFF) == 0x00000804)
+ {
+ Result = DEVICETYPE_NXT;
+ }
+ if ((COD & 0x00001F00) == 0x00000200)
+ {
+ Result = DEVICETYPE_PHONE;
+ }
+ if ((COD & 0x00001F00) == 0x00000100)
+ {
+ Result = DEVICETYPE_PC;
+ }
+
+ return (Result);
+}
+
+
+UBYTE cUiBTGetDeviceIndex(UBYTE Known,UBYTE No,UBYTE *pIndex)
+{
+ UBYTE Result = 0;
+ UBYTE Tmp;
+
+ *pIndex = 0;
+ if (Known)
+ {
+ for (Tmp = 0;(Tmp < SIZE_OF_BT_DEVICE_TABLE) && (Result == 0);Tmp++)
+ {
+ if ((pMapComm->BtDeviceTable[Tmp].DeviceStatus & BT_DEVICE_KNOWN))
+ {
+ if (No == *pIndex)
+ {
+ *pIndex = Tmp;
+ Result = ~0;
+ }
+ else
+ {
+ (*pIndex)++;
+ }
+ }
+ }
+ }
+ else
+ {
+ for (Tmp = 0;(Tmp < SIZE_OF_BT_DEVICE_TABLE) && (Result == 0);Tmp++)
+ {
+ if ((pMapComm->BtDeviceTable[Tmp].DeviceStatus & BT_DEVICE_UNKNOWN) || (pMapComm->BtDeviceTable[Tmp].DeviceStatus & BT_DEVICE_KNOWN))
+ {
+ if (No == *pIndex)
+ {
+ *pIndex = Tmp;
+ Result = ~0;
+ }
+ else
+ {
+ (*pIndex)++;
+ }
+ }
+ }
+ }
+
+ return (Result);
+}
+
+
+UWORD cUiBTCommand(UBYTE Cmd,UBYTE Flag,UBYTE *pParam1,UBYTE *pParam2)
+{
+ UWORD Result = UI_BT_FAILED;
+
+ switch(Cmd)
+ {
+ case UI_BT_GET_DEVICES :
+ {
+ cUiBTGetDeviceIndex(Flag,SIZE_OF_BT_DEVICE_TABLE,pParam1);
+ Result = UI_BT_SUCCES;
+ }
+ break;
+
+ case UI_BT_GET_DEVICE_NAME :
+ {
+ if ((*pParam1 < SIZE_OF_BT_DEVICE_TABLE) && (pParam2 != NULL))
+ {
+ pParam2[0] = 0;
+ if (cUiBTGetDeviceIndex(Flag,*pParam1,&VarsUi.BTTmpIndex))
+ {
+ sprintf((char*)pParam2,"%.*s",DISPLAYLINE_LENGTH,(char*)pMapComm->BtDeviceTable[VarsUi.BTTmpIndex].Name);
+ Result = UI_BT_SUCCES;
+ }
+ }
+ }
+ break;
+
+ case UI_BT_GET_DEVICE_TYPE :
+ {
+ if ((*pParam1 < SIZE_OF_BT_DEVICE_TABLE) && (pParam2 != NULL))
+ {
+ pParam2[0] = 0;
+ if (cUiBTGetDeviceIndex(Flag,*pParam1,&VarsUi.BTTmpIndex))
+ {
+ pParam2[0] = cUiBTGetDeviceType(pMapComm->BtDeviceTable[VarsUi.BTTmpIndex].ClassOfDevice);
+ Result = UI_BT_SUCCES;
+ }
+ }
+ }
+ break;
+
+ case UI_BT_GET_CONNECTION_NAME :
+ {
+ if (*pParam1 < SIZE_OF_BT_CONNECT_TABLE)
+ {
+ if (pMapComm->BtConnectTable[*pParam1].Name[0])
+ {
+ if (pParam2 != NULL)
+ {
+ sprintf((char*)pParam2,"%.*s",DISPLAYLINE_LENGTH,(char*)pMapComm->BtConnectTable[*pParam1].Name);
+ }
+ Result = UI_BT_SUCCES;
+ }
+ else
+ {
+ if (pParam2 != NULL)
+ {
+ pParam2[0] = 0;
+ }
+ }
+ }
+ }
+ break;
+
+ case UI_BT_GET_CONNECTION_TYPE :
+ {
+ if ((*pParam1 < SIZE_OF_BT_CONNECT_TABLE) && (pParam2 != NULL))
+ {
+ pParam2[0] = 0;
+ if (pMapComm->BtConnectTable[*pParam1].Name[0])
+ {
+ pParam2[0] = cUiBTGetDeviceType(pMapComm->BtConnectTable[*pParam1].ClassOfDevice);
+ Result = UI_BT_SUCCES;
+ }
+ }
+ }
+ break;
+
+ case UI_BT_GET_CONNECTION_VALID :
+ {
+ if (*pParam1 < SIZE_OF_BT_CONNECT_TABLE)
+ {
+ if (pMapComm->BtConnectTable[*pParam1].Name[0])
+ {
+ Result = UI_BT_SUCCES;
+ }
+ }
+ }
+ break;
+
+ }
+
+ return (Result);
+}
+
+
+
+#include "BtTest.inc"
+
+//******* cUiNVxxxxx *********************************************************
+
+void cUiNVWrite(void)
+{
+ sprintf((char*)VarsUi.NVFilename,"%s.%s",(char*)NONVOLATILE_NAME,(char*)TXT_SYS_EXT);
+ VarsUi.NVTmpHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.NVFilename,VarsUi.SearchFilenameBuffer,&VarsUi.NVTmpLength);
+ if (!(VarsUi.NVTmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.NVTmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.NVFilename,NULL,NULL);
+ }
+ VarsUi.NVTmpLength = sizeof(NVDATA);
+ VarsUi.NVTmpHandle = pMapLoader->pFunc(OPENWRITE,VarsUi.NVFilename,NULL,&VarsUi.NVTmpLength);
+ pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.NVTmpHandle,(UBYTE*)&VarsUi.NVData,&VarsUi.NVTmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.NVTmpHandle,NULL,NULL);
+}
+
+
+void cUiNVRead(void)
+{
+ VarsUi.NVData.CheckByte = 0;
+ sprintf((char*)VarsUi.NVFilename,"%s.%s",(char*)NONVOLATILE_NAME,(char*)TXT_SYS_EXT);
+ VarsUi.NVTmpHandle = pMapLoader->pFunc(OPENREAD,VarsUi.NVFilename,NULL,&VarsUi.NVTmpLength);
+ if (!(VarsUi.NVTmpHandle & 0x8000))
+ {
+ VarsUi.NVTmpLength = sizeof(NVDATA);
+ pMapLoader->pFunc(READ,(UBYTE*)&VarsUi.NVTmpHandle,(UBYTE*)&VarsUi.NVData,&VarsUi.NVTmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.NVTmpHandle,NULL,NULL);
+ }
+ if (VarsUi.NVData.CheckByte != CHECKBYTE)
+ {
+ VarsUi.NVData.DatalogEnabled = DATALOGENABLED;
+ VarsUi.NVData.VolumeStep = MAX_VOLUME;
+ VarsUi.NVData.PowerdownCode = POWER_OFF_TIME_DEFAULT;
+ VarsUi.NVData.DatalogNumber = 0;
+ VarsUi.NVData.CheckByte = CHECKBYTE;
+ cUiNVWrite();
+ }
+}
+
+
+//******* cUiFeedback ********************************************************
+
+UBYTE cUiFeedback(BMPMAP *Bitmap,UBYTE TextNo1,UBYTE TextNo2,UWORD Time) // Show bimap and text
+{
+// if ((VarsUi.FBState == 0) || ((pMapDisplay->Flags & DISPLAY_POPUP) == 0))
+ {
+ switch (VarsUi.FBState)
+ {
+ case 0 : // Set busy
+ {
+ VarsUi.FBState++;
+ }
+ break;
+
+ case 1 : // Clear line 2,3,4
+ {
+ if (DISPLAY_IDLE)
+ {
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_2) | TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4));
+ VarsUi.FBState++;
+ }
+ }
+ break;
+
+ case 2 : // Show bitmap if pressent
+ {
+ if (DISPLAY_IDLE)
+ {
+ if (Bitmap != NULL)
+ {
+ pMapDisplay->pBitmaps[BITMAP_1] = Bitmap;
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_1);
+ }
+ VarsUi.FBState++;
+ }
+ }
+ break;
+
+ case 3 : // Get text string
+ {
+ if (DISPLAY_IDLE)
+ {
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(TOPLINE);
+ VarsUi.FBText = cUiGetString(TextNo1);
+ VarsUi.FBPointer = 0;
+ if (TextNo2)
+ {
+ VarsUi.FBState = 5;
+ }
+ else
+ {
+ VarsUi.FBState++;
+ }
+ }
+ }
+ break;
+
+ case 4 : // Show text string
+ {
+ if ((VarsUi.FBText[VarsUi.FBPointer]) && (VarsUi.FBPointer < NO_OF_FEEDBACK_CHARS))
+ {
+ pMapDisplay->pFunc(DISPLAY_CHAR,TRUE,24 + VarsUi.FBPointer * 6,16,VarsUi.FBText[VarsUi.FBPointer],0);
+ VarsUi.FBPointer++;
+ }
+ else
+ {
+ VarsUi.FBTimer = 0;
+ VarsUi.FBState = 7;
+ }
+ }
+ break;
+
+ case 5 : // Show text string
+ {
+ if ((VarsUi.FBText[VarsUi.FBPointer]) && (VarsUi.FBPointer < NO_OF_FEEDBACK_CHARS))
+ {
+ pMapDisplay->pFunc(DISPLAY_CHAR,TRUE,24 + VarsUi.FBPointer * 6,12,VarsUi.FBText[VarsUi.FBPointer],0);
+ VarsUi.FBPointer++;
+ }
+ else
+ {
+ if (TextNo2 == 0xFF)
+ {
+ VarsUi.FBText = VarsUi.SelectedFilename;
+ }
+ else
+ {
+ VarsUi.FBText = cUiGetString(TextNo2);
+ }
+ VarsUi.FBPointer = 0;
+ VarsUi.FBState++;
+ }
+ }
+ break;
+
+ case 6 : // Show text string
+ {
+ if ((VarsUi.FBText[VarsUi.FBPointer]) && (VarsUi.FBPointer < NO_OF_FEEDBACK_CHARS))
+ {
+ pMapDisplay->pFunc(DISPLAY_CHAR,TRUE,24 + VarsUi.FBPointer * 6,20,VarsUi.FBText[VarsUi.FBPointer],0);
+ VarsUi.FBPointer++;
+ }
+ else
+ {
+ VarsUi.FBTimer = 0;
+ VarsUi.FBState++;
+ }
+ }
+ break;
+
+ case 7 : // Wait if time provided
+ {
+ if (++VarsUi.FBTimer >= (Time + 100))
+ {
+ VarsUi.FBState++;
+ }
+ }
+ break;
+
+ default : // Exit
+ {
+ VarsUi.FBState = 0;
+ }
+ break;
+
+ }
+ }
+
+ return (VarsUi.FBState);
+}
+
+
+
+//******* cUiFileList ********************************************************
+
+UBYTE cUiFindNoOfFiles(UBYTE FileType,UBYTE *NoOfFiles)
+{
+ switch (VarsUi.FNOFState)
+ {
+ case 0 :
+ {
+ *NoOfFiles = 0;
+
+ if (FileType >= FILETYPES)
+ {
+ FileType = FILETYPE_ALL;
+ }
+ sprintf((char*)VarsUi.FNOFSearchBuffer,"*.%s",TXT_FILE_EXT[FileType]);
+
+ VarsUi.FNOFHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FNOFSearchBuffer,VarsUi.FNOFNameBuffer,&VarsUi.FNOFLength);
+ if (!(VarsUi.FNOFHandle & 0x8000))
+ {
+ *NoOfFiles = 1;
+ VarsUi.FNOFState++;
+ }
+ }
+ break;
+
+ case 1 :
+ {
+ VarsUi.FNOFHandle = pMapLoader->pFunc(FINDNEXT,(UBYTE*)&VarsUi.FNOFHandle,VarsUi.FNOFNameBuffer,&VarsUi.FNOFLength);
+ if (!(VarsUi.FNOFHandle & 0x8000))
+ {
+ *NoOfFiles += 1;
+ }
+ else
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.FNOFHandle,NULL,NULL);
+ VarsUi.FNOFState = 0;
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.FNOFState);
+}
+
+
+UBYTE cUiFindNameForFileNo(UBYTE FileType,UBYTE FileNo,UBYTE *Name)
+{
+ switch (VarsUi.FNOFState)
+ {
+ case 0 :
+ {
+ Name[0] = 0;
+
+ if (FileNo)
+ {
+ if (FileType >= FILETYPES)
+ {
+ FileType = FILETYPE_ALL;
+ }
+ sprintf((char*)VarsUi.FNOFSearchBuffer,"*.%s",TXT_FILE_EXT[FileType]);
+
+ VarsUi.FNOFHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FNOFSearchBuffer,Name,&VarsUi.FNOFLength);
+ if (!(VarsUi.FNOFHandle & 0x8000))
+ {
+ if (FileNo != 1)
+ {
+ VarsUi.FNOFFileNo = 1;
+ VarsUi.FNOFState++;
+ }
+ else
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.FNOFHandle,NULL,NULL);
+ }
+ }
+ }
+ }
+ break;
+
+ case 1 :
+ {
+ VarsUi.FNOFHandle = pMapLoader->pFunc(FINDNEXT,(UBYTE*)&VarsUi.FNOFHandle,Name,&VarsUi.FNOFLength);
+ if (!(VarsUi.FNOFHandle & 0x8000))
+ {
+ VarsUi.FNOFFileNo++;
+ if (FileNo == VarsUi.FNOFFileNo)
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.FNOFHandle,NULL,NULL);
+ VarsUi.FNOFState = 0;
+ }
+ }
+ else
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.FNOFHandle,NULL,NULL);
+ VarsUi.FNOFState = 0;
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.FNOFState);
+}
+
+
+UBYTE cUiFileList(UBYTE Action) // Show files and select
+{
+ switch (Action)
+ {
+ case MENU_INIT :
+ {
+ if (!VarsUi.State)
+ {
+ VarsUi.FileCenter = 1;
+ VarsUi.NextState = IOMapUi.State;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_LEFT :
+ {
+ if (!VarsUi.State)
+ {
+ cUiListLeft(VarsUi.NoOfFiles,&VarsUi.FileCenter);
+ VarsUi.NextState = TEST_BUTTONS;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_RIGHT :
+ {
+ if (!VarsUi.State)
+ {
+ cUiListRight(VarsUi.NoOfFiles,&VarsUi.FileCenter);
+ VarsUi.NextState = TEST_BUTTONS;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_SELECT :
+ {
+ }
+ break;
+
+ default :
+ {
+ if (Action < FILETYPES)
+ {
+ if (!VarsUi.State)
+ {
+ VarsUi.FileType = Action;
+ VarsUi.FileCenter = 1;
+ VarsUi.NextState = IOMapUi.State;
+ }
+ Action = MENU_DRAW;
+ }
+ else
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+
+ if (Action == MENU_DRAW)
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.FNOFState = 0;
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+ if (cUiFindNoOfFiles(VarsUi.FileType,&VarsUi.NoOfFiles) == 0)
+ {
+ if (VarsUi.NoOfFiles)
+ {
+ cUiListCalc(VarsUi.NoOfFiles,&VarsUi.FileCenter,&VarsUi.FileLeft,&VarsUi.FileRight);
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ if (cUiFindNameForFileNo(VarsUi.FileType,VarsUi.FileCenter,VarsUi.SelectedFilename) == 0)
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default :
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = NULL;
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = NULL;
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = NULL;
+
+ if (VarsUi.FileLeft)
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Icons->Data[(VarsUi.FileType + ALLFILES) * Icons->ItemPixelsX * (Icons->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_LEFT);
+ }
+ if (VarsUi.FileCenter)
+ {
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Icons->Data[(VarsUi.FileType + ALLFILES) * Icons->ItemPixelsX * (Icons->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_CENTER);
+ }
+ if (VarsUi.FileRight)
+ {
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Icons->Data[(VarsUi.FileType + ALLFILES) * Icons->ItemPixelsX * (Icons->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_RIGHT);
+ }
+
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_5);
+
+ // Search forward for termination
+ VarsUi.Tmp = 0;
+ while ((VarsUi.SelectedFilename[VarsUi.Tmp]) && (VarsUi.Tmp < FILENAME_LENGTH))
+ {
+ VarsUi.Tmp++;
+ }
+
+ // Search backward for "."
+ while ((VarsUi.Tmp) && (VarsUi.SelectedFilename[VarsUi.Tmp] != '.'))
+ {
+ VarsUi.Tmp--;
+ }
+
+ if (VarsUi.Tmp > DISPLAYLINE_LENGTH)
+ {
+ VarsUi.Tmp = DISPLAYLINE_LENGTH;
+ }
+
+ VarsUi.DisplayBuffer[VarsUi.Tmp] = 0;
+
+ // Copy only name not ext
+ while (VarsUi.Tmp)
+ {
+ VarsUi.Tmp--;
+ VarsUi.DisplayBuffer[VarsUi.Tmp] = VarsUi.SelectedFilename[VarsUi.Tmp];
+ }
+
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+ pMapDisplay->EraseMask |= MENUICON_BITS;
+ pMapDisplay->UpdateMask |= (SPECIAL_BIT(FRAME_SELECT) | SPECIAL_BIT(MENUTEXT));
+
+ IOMapUi.State = VarsUi.NextState;
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiVolume **********************************************************
+
+UBYTE cUiVolume(UBYTE Action) // MENU_INIT,MENU_LEFT,MENU_RIGHT,MENU_EXIT
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Init time counter and cursor bitmap
+ {
+ VarsUi.Counter = VarsUi.NVData.VolumeStep + 1;
+
+ VarsUi.pTmp = (UBYTE*)Cursor;
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < SIZE_OF_CURSOR) && (VarsUi.Tmp < (UBYTE)SIZEOF_DATA(Cursor));VarsUi.Tmp++)
+ {
+ VarsUi.CursorTmp[VarsUi.Tmp] = *VarsUi.pTmp;
+ VarsUi.pTmp++;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_LEFT : // Dec
+ {
+ cUiListLeft(MAX_VOLUME + 1,&VarsUi.Counter);
+ IOMapUi.Volume = VarsUi.Counter - 1;
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_RIGHT : // Inc
+ {
+ cUiListRight(MAX_VOLUME + 1,&VarsUi.Counter);
+ IOMapUi.Volume = VarsUi.Counter - 1;
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_ENTER : // Enter
+ {
+ VarsUi.NVData.VolumeStep = VarsUi.Counter - 1;
+ cUiNVWrite();
+ IOMapUi.Volume = VarsUi.NVData.VolumeStep;
+ pMapSound->Volume = IOMapUi.Volume;
+ Action = MENU_EXIT;
+ }
+ break;
+
+ case MENU_EXIT : // Leave
+ {
+ IOMapUi.Volume = VarsUi.NVData.VolumeStep;
+ }
+ break;
+
+ }
+ if (Action == MENU_DRAW)
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,"%u",(UWORD)VarsUi.Counter - 1);
+ pMapDisplay->pTextLines[TEXTLINE_3] = VarsUi.DisplayBuffer;
+
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)VarsUi.CursorTmp;
+ VarsUi.CursorTmp[4] = 46;
+ VarsUi.CursorTmp[5] = 24;
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4));
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(TEXTLINE_3) | BITMAP_BIT(BITMAP_1));
+ }
+ if (Action == MENU_EXIT)
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+ return (0);
+}
+
+
+
+//******* cUiGetUserString ***************************************************
+
+#define STRINGTYPES 2
+
+#define TOPTEXT_LINE TEXTLINE_3
+#define STRING_LINE TEXTLINE_5
+
+typedef struct
+{
+ const UBYTE Text;
+ const UBYTE *Figures;
+ const UBYTE NoOfFigures;
+ const UBYTE MaxStringLength;
+ const UBYTE WindowSize;
+ const SBYTE DefaultPointer;
+}
+STRSETS;
+
+const UBYTE Figures[] = { "0987654321" "\x7F" "abcdefghijklmnopqrstuvwxyz " };
+
+const STRSETS StrSets[STRINGTYPES] =
+{
+ { TXT_GETUSERSTRING_PIN, Figures, 37, SIZE_OF_BT_PINCODE - 1, 15, 10 },
+ { TXT_GETUSERSTRING_FILENAME, Figures, 37, FILENAME_LENGTH - 4 , 15, 10 }
+};
+
+
+UBYTE cUiGetUserString(UBYTE Type) // 0=Pincode, 1=filename
+{
+ UBYTE Tmp1;
+ SBYTE Tmp2;
+
+ if (Type < STRINGTYPES)
+ {
+ switch (VarsUi.GUSState)
+ {
+ case 0 : // Init screen
+ {
+ // Disable update and prepare screen
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)Ok;
+
+ // Set figure pointer to default
+ VarsUi.FigurePointer = (SBYTE)StrSets[Type].DefaultPointer;
+
+ // Calculate cursor from default string
+ VarsUi.GUSCursor = 0;
+ while ((VarsUi.GUSCursor < DISPLAYLINE_LENGTH) && VarsUi.UserString[VarsUi.GUSCursor])
+ {
+ VarsUi.GUSCursor++;
+ }
+ VarsUi.GUSNoname = TRUE;
+
+ VarsUi.GUSState++;
+ }
+ break;
+
+ case 1 : // Update user string
+ {
+ if (!(pMapDisplay->EraseMask & SCREEN_BIT(SCREEN_LARGE)))
+ {
+ // Display top text
+ pMapDisplay->pTextLines[TOPTEXT_LINE] = cUiGetString(StrSets[Type].Text);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TOPTEXT_LINE);
+
+ Tmp1 = 0;
+ while (VarsUi.UserString[Tmp1] && (Tmp1 < StrSets[Type].MaxStringLength))
+ {
+ VarsUi.DisplayText[Tmp1] = VarsUi.UserString[Tmp1];
+ Tmp1++;
+ }
+ if (Tmp1 < StrSets[Type].MaxStringLength)
+ {
+ VarsUi.DisplayText[Tmp1] = '_';
+ Tmp1++;
+ }
+ while (Tmp1 < StrSets[Type].MaxStringLength)
+ {
+ VarsUi.DisplayText[Tmp1] = ' ';
+ Tmp1++;
+ }
+ VarsUi.DisplayText[Tmp1] = 0;
+
+ pMapDisplay->pTextLines[STRING_LINE] = VarsUi.DisplayText;
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(STRING_LINE) | SPECIAL_BIT(TOPLINE));
+ pMapDisplay->EraseMask |= BITMAP_BIT(BITMAP_1);
+ VarsUi.GUSState++;
+ }
+ }
+ break;
+
+ case 2 : // Update figure string
+ {
+ if (!(pMapDisplay->EraseMask & BITMAP_BIT(BITMAP_1)))
+ {
+ Tmp2 = VarsUi.FigurePointer;
+
+ for (Tmp1 = 0;Tmp1 < 3;Tmp1++)
+ {
+ if (Tmp2)
+ {
+ Tmp2--;
+ }
+ else
+ {
+ Tmp2 = StrSets[Type].NoOfFigures - 1;
+ }
+ }
+ for (Tmp1 = 0;Tmp1 < 7;Tmp1++)
+ {
+ if ((Tmp1 == 3) && (StrSets[Type].Figures[Tmp2] == 0x7F))
+ {
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_1);
+ }
+ else
+ {
+ pMapDisplay->pFunc(DISPLAY_CHAR,TRUE,5 + Tmp1 * 14,52,StrSets[Type].Figures[Tmp2],0);
+ }
+ if (Tmp2 < (StrSets[Type].NoOfFigures - 1))
+ {
+ Tmp2++;
+ }
+ else
+ {
+ Tmp2 = 0;
+ }
+ }
+ pMapDisplay->pFunc(DISPLAY_HORISONTAL_LINE,TRUE,42,47,57,0);
+ pMapDisplay->pFunc(DISPLAY_VERTICAL_LINE,TRUE,42,47,0,63);
+ pMapDisplay->pFunc(DISPLAY_VERTICAL_LINE,TRUE,57,47,0,63);
+
+ VarsUi.GUSState++;
+ }
+ }
+ break;
+
+ case 3 : // Get user input
+ {
+ if ((pMapButton->State[BTN4] & LONG_PRESSED_EV))
+ {
+ if (VarsUi.GUSCursor)
+ {
+ if ((VarsUi.UserString[VarsUi.GUSCursor - 1] >= 'a') && (VarsUi.UserString[VarsUi.GUSCursor - 1] <= 'z'))
+ {
+ VarsUi.UserString[VarsUi.GUSCursor - 1] -= ' ';
+ VarsUi.GUSState -= 2;
+ }
+ }
+ }
+
+ switch (cUiReadButtons())
+ {
+ case BUTTON_LEFT :
+ {
+ if (VarsUi.FigurePointer)
+ {
+ VarsUi.FigurePointer--;
+ }
+ else
+ {
+ VarsUi.FigurePointer = StrSets[Type].NoOfFigures - 1;
+ }
+ pMapDisplay->EraseMask |= BITMAP_BIT(BITMAP_1);
+ VarsUi.GUSState -= 2;
+ }
+ break;
+
+ case BUTTON_ENTER :
+ {
+ switch (StrSets[Type].Figures[VarsUi.FigurePointer])
+ {
+ case 0x7F :
+ {
+ VarsUi.GUSState = 100;
+ }
+ break;
+
+ default :
+ {
+ VarsUi.GUSNoname = FALSE;
+ if (VarsUi.GUSCursor < StrSets[Type].MaxStringLength)
+ {
+ VarsUi.UserString[VarsUi.GUSCursor] = StrSets[Type].Figures[VarsUi.FigurePointer];
+ VarsUi.GUSCursor++;
+ VarsUi.UserString[VarsUi.GUSCursor] = 0;
+ VarsUi.GUSState -= 2;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ if (VarsUi.FigurePointer < (StrSets[Type].NoOfFigures - 1))
+ {
+ VarsUi.FigurePointer++;
+ }
+ else
+ {
+ VarsUi.FigurePointer = 0;
+ }
+ pMapDisplay->EraseMask |= BITMAP_BIT(BITMAP_1);
+ VarsUi.GUSState -= 2;
+ }
+ break;
+
+ case BUTTON_EXIT :
+ {
+ if (VarsUi.GUSCursor)
+ {
+ if (VarsUi.GUSNoname == TRUE)
+ {
+ VarsUi.GUSNoname = FALSE;
+ while (VarsUi.GUSCursor)
+ {
+ VarsUi.UserString[VarsUi.GUSCursor] = 0;
+ VarsUi.GUSCursor--;
+ }
+ }
+ else
+ {
+ VarsUi.GUSCursor--;
+ }
+ VarsUi.UserString[VarsUi.GUSCursor] = 0;
+ VarsUi.GUSState -= 2;
+ }
+ else
+ {
+ VarsUi.UserString[0] = 0;
+ VarsUi.GUSState = 100;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ default : // Clean up screen
+ {
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_BACKGROUND);
+ pMapDisplay->UpdateMask = 0;
+ IOMapUi.Flags |= UI_REDRAW_STATUS;
+ VarsUi.GUSState = 0;
+ }
+ break;
+ }
+ }
+
+ return (VarsUi.GUSState);
+}
+
+
+
+//******* cUiDataLogging *****************************************************
+
+void cUiDrawPortNo(UBYTE *Bitmap,UBYTE MenuIconNo,UBYTE PortNo)
+{
+ UBYTE Tmp;
+
+ Bitmap[0] = (UBYTE)(FILEFORMAT_BITMAP >> 8);
+ Bitmap[1] = (UBYTE)(FILEFORMAT_BITMAP);
+ Bitmap[2] = (UBYTE)(SIZE_OF_PORTBITMAP >> 8);
+ Bitmap[3] = (UBYTE)(SIZE_OF_PORTBITMAP);
+ Bitmap[4] = DISPLAY_MENUICONS_X_OFFS + DISPLAY_MENUICONS_X_DIFF * MenuIconNo + 2;
+ Bitmap[5] = DISPLAY_MENUICONS_Y;
+ Bitmap[6] = Port[0].ItemPixelsX;
+ Bitmap[7] = Port[0].ItemPixelsY;
+
+ Tmp = 0;
+ while (Tmp < Bitmap[6])
+ {
+ Bitmap[Tmp + FILEHEADER_LENGTH] = Port[0].Data[Tmp + PortNo * Bitmap[6]];
+ Tmp++;
+ }
+
+}
+
+UBYTE cUiDataLogging(UBYTE Action)
+{
+ SBYTE TmpBuffer[DATALOGBUFFERSIZE + 1];
+
+ switch (Action)
+ {
+ case MENU_INIT : // Initialize all ports to empty
+ {
+// Show select
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_VIEW_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+
+// Init ports
+ for (VarsUi.Tmp = 0;VarsUi.Tmp < DATALOGPORTS;VarsUi.Tmp++)
+ {
+ VarsUi.DatalogPort[VarsUi.Tmp] = MENU_SENSOR_EMPTY;
+ }
+ }
+ break;
+
+ case MENU_EXIT : // Initialize all ports to empty
+ {
+// Show select
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_VIEW_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+ }
+ break;
+
+ case MENU_TEXT : // Write text
+ {
+// Init selected sensor and port to none
+ VarsUi.SelectedSensor = MENU_SENSOR_EMPTY;
+ VarsUi.SelectedPort = MENU_PORT_EMPTY;
+// Count ports
+ VarsUi.Counter = 0;
+ for (VarsUi.Tmp = 0;VarsUi.Tmp < DATALOGPORTS;VarsUi.Tmp++)
+ {
+ if (MENU_SENSOR_EMPTY != VarsUi.DatalogPort[VarsUi.Tmp])
+ {
+// Find default port to view
+ if (VarsUi.SelectedPort == MENU_PORT_EMPTY)
+ {
+ VarsUi.SelectedPort = VarsUi.Tmp + MENU_PORT_1;
+ }
+ VarsUi.Counter++;
+ }
+ }
+ if (VarsUi.Counter)
+ {
+// Display text
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_DATALOGGING_PRESS_EXIT_TO);
+ pMapDisplay->pTextLines[TEXTLINE_4] = cUiGetString(TXT_DATALOGGING_STOP_DATALOGGING);
+
+ pMapDisplay->TextLinesCenterFlags |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4));
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4));
+ }
+ else
+ {
+ cUiMenuPrevFile();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ case MENU_RUN : // Run data logging
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Init log
+ {
+// Save menu text
+ VarsUi.MenuIconTextSave = pMapDisplay->pMenuText;
+
+// Delete file if exist
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_DATALOG_FILENAME,(char*)TXT_FILE_EXT[FILETYPE_DATALOG]);
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FilenameBuffer,VarsUi.SearchFilenameBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.FilenameBuffer,NULL,NULL);
+ }
+
+// Open file
+ VarsUi.TmpLength = pMapLoader->FreeUserFlash;
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_DATALOG_FILENAME,(char*)TXT_FILE_EXT[FILETYPE_DATALOG]);
+ VarsUi.TmpHandle = pMapLoader->pFunc(OPENWRITEDATA,VarsUi.FilenameBuffer,NULL,&VarsUi.TmpLength);
+ VarsUi.DatalogError = VarsUi.TmpHandle;
+ if (!(VarsUi.DatalogError & 0x8000))
+ {
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"%s\t%lu",SENSORSYNCDATA,pMapCmd->SyncTime);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t%lu",pMapCmd->SyncTick);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t%lu",pMapCmd->Tick);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t%lu\t-1\r\n",DATALOG_DEFAULT_SAMPLE_TIME);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"%s",SENSORSDATA);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < DATALOGPORTS) && (!(VarsUi.DatalogError & 0x8000));VarsUi.Tmp++)
+ {
+ if (MENU_SENSOR_EMPTY != VarsUi.DatalogPort[VarsUi.Tmp])
+ {
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t%u_%s%s",(UWORD)(VarsUi.Tmp + 1),(char*)SENSORDIRNAME[(VarsUi.DatalogPort[VarsUi.Tmp] - MENU_SENSOR_EMPTY) - 1],(char*)SENSORUNITNAME[(VarsUi.DatalogPort[VarsUi.Tmp] - MENU_SENSOR_EMPTY) - 1]);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ }
+ }
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\r\n");
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"%s",SENSORTIME);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < DATALOGPORTS) && (!(VarsUi.DatalogError & 0x8000));VarsUi.Tmp++)
+ {
+ if (MENU_SENSOR_EMPTY != VarsUi.DatalogPort[VarsUi.Tmp])
+ {
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t%s",(char*)SENSORDIRNAME[(VarsUi.DatalogPort[VarsUi.Tmp] - MENU_SENSOR_EMPTY) - 1]);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ }
+ }
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\r\n");
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.DatalogError & 0x8000))
+ {
+ VarsUi.DatalogTimer = 0;
+ VarsUi.DatalogSampleTime = DATALOG_DEFAULT_SAMPLE_TIME;
+ VarsUi.DatalogSampleTimer = 0;
+ VarsUi.Timer = 0;
+ VarsUi.Update = TRUE;
+ IOMapUi.Flags |= UI_BUSY;
+ VarsUi.DatalogOldTick = pMapCmd->Tick;
+ VarsUi.SensorReset = TRUE;
+ VarsUi.State++;
+ }
+ else
+ {
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+ pMapDisplay->pBitmaps[BITMAP_1] = NULL;
+ VarsUi.State = 4;
+ }
+ }
+ else
+ {
+// File error
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+ pMapDisplay->pBitmaps[BITMAP_1] = NULL;
+ VarsUi.State = 3;
+ }
+ }
+ break;
+
+ case 1 :
+ {
+// Get real time since last
+ VarsUi.DatalogRTC = (pMapCmd->Tick - VarsUi.DatalogOldTick);
+ VarsUi.DatalogOldTick = pMapCmd->Tick;
+// Update all timers
+ VarsUi.DatalogTimer += VarsUi.DatalogRTC;
+ VarsUi.DatalogSampleTimer += VarsUi.DatalogRTC;
+ VarsUi.ReadoutTimer += VarsUi.DatalogRTC;
+// Update sensor values
+ cUiUpdateSensor((SWORD)VarsUi.DatalogRTC);
+// Check for select change
+ if (VarsUi.Update == TRUE)
+ {
+ VarsUi.Update = FALSE;
+ VarsUi.SelectedSensor = VarsUi.DatalogPort[VarsUi.SelectedPort - MENU_PORT_1];
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = cUiMenuGetIconImage(cUiMenuSearchSensorIcon(VarsUi.SelectedSensor));
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = NULL;
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = NULL;
+
+ pMapDisplay->EraseMask = SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)Display;
+ pMapDisplay->UpdateMask = (BITMAP_BIT(BITMAP_1) | MENUICON_BITS | SPECIAL_BIT(TOPLINE) | SPECIAL_BIT(FRAME_SELECT));
+
+ pMapDisplay->pBitmaps[BITMAP_2] = (BMPMAP*)VarsUi.PortBitmapLeft;
+ pMapDisplay->pBitmaps[BITMAP_3] = (BMPMAP*)VarsUi.PortBitmapCenter;
+ pMapDisplay->pBitmaps[BITMAP_4] = (BMPMAP*)VarsUi.PortBitmapRight;
+
+ cUiDrawPortNo(VarsUi.PortBitmapCenter,MENUICON_CENTER,VarsUi.SelectedPort - MENU_PORT_EMPTY);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_3);
+
+
+
+ if (VarsUi.Counter == 2)
+ {
+ VarsUi.Tmp = VarsUi.SelectedPort;
+ do
+ {
+ VarsUi.Tmp++;
+ if (VarsUi.Tmp >= MENU_PORT_INVALID)
+ {
+ VarsUi.Tmp = MENU_PORT_1;
+ }
+ }
+ while (VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1] == MENU_SENSOR_EMPTY);
+ if (VarsUi.Tmp > VarsUi.SelectedPort)
+ {
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = cUiMenuGetIconImage(cUiMenuSearchSensorIcon(VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1]));
+ cUiDrawPortNo(VarsUi.PortBitmapRight,MENUICON_RIGHT,VarsUi.Tmp - MENU_PORT_EMPTY);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_4);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = cUiMenuGetIconImage(cUiMenuSearchSensorIcon(VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1]));
+ cUiDrawPortNo(VarsUi.PortBitmapLeft,MENUICON_LEFT,VarsUi.Tmp - MENU_PORT_EMPTY);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_2);
+ }
+ }
+ if (VarsUi.Counter > 2)
+ {
+ VarsUi.Tmp = VarsUi.SelectedPort;
+ do
+ {
+ VarsUi.Tmp++;
+ if (VarsUi.Tmp >= MENU_PORT_INVALID)
+ {
+ VarsUi.Tmp = MENU_PORT_1;
+ }
+ }
+ while (VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1] == MENU_SENSOR_EMPTY);
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = cUiMenuGetIconImage(cUiMenuSearchSensorIcon(VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1]));
+ cUiDrawPortNo(VarsUi.PortBitmapRight,MENUICON_RIGHT,VarsUi.Tmp - MENU_PORT_EMPTY);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_4);
+
+ VarsUi.Tmp = VarsUi.SelectedPort;
+ do
+ {
+ VarsUi.Tmp--;
+ if (VarsUi.Tmp <= MENU_PORT_EMPTY)
+ {
+ VarsUi.Tmp = MENU_PORT_INVALID - 1;
+ }
+ }
+ while (VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1] == MENU_SENSOR_EMPTY);
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = cUiMenuGetIconImage(cUiMenuSearchSensorIcon(VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1]));
+ cUiDrawPortNo(VarsUi.PortBitmapLeft,MENUICON_LEFT,VarsUi.Tmp - MENU_PORT_EMPTY);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_2);
+
+
+ }
+ VarsUi.ReadoutTimer = DISPLAY_VIEW_UPDATE;
+ }
+// Write sample if timeout
+ if (VarsUi.DatalogSampleTimer >= VarsUi.DatalogSampleTime)
+ {
+ VarsUi.DatalogSampleTimer -= VarsUi.DatalogSampleTime;
+
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"%lu",VarsUi.DatalogTimer - VarsUi.DatalogSampleTime);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < DATALOGPORTS) && (!(VarsUi.DatalogError & 0x8000));VarsUi.Tmp++)
+ {
+ if (MENU_SENSOR_EMPTY != VarsUi.DatalogPort[VarsUi.Tmp])
+ {
+ if (VarsUi.DatalogSampleValid[VarsUi.Tmp] == TRUE)
+ {
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,(char*)SENSORFORMAT2[(VarsUi.DatalogPort[VarsUi.Tmp] - MENU_SENSOR_EMPTY) - 1],(float)VarsUi.DatalogSampleValue[VarsUi.Tmp] / SENSORDIVIDER[VarsUi.DatalogPort[VarsUi.Tmp] - MENU_SENSOR_EMPTY]);
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ }
+ else
+ {
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\t-");
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ }
+ }
+ }
+ VarsUi.TmpLength = (ULONG)sprintf((char*)TmpBuffer,"\r\n");
+ VarsUi.DatalogError |= pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)TmpBuffer,&VarsUi.TmpLength);
+ }
+// Refresh display
+ if (++VarsUi.ReadoutTimer >= DISPLAY_VIEW_UPDATE)
+ {
+ VarsUi.ReadoutTimer = 0;
+
+// Display sensor value
+ cUiPrintSensorInDisplayBuffer(VarsUi.SelectedPort);
+ pMapDisplay->pTextLines[TEXTLINE_4] = VarsUi.DisplayBuffer;
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4);
+ }
+
+// Test for file error
+ if ((VarsUi.DatalogError & 0x8000))
+ {
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+ pMapDisplay->pBitmaps[BITMAP_1] = NULL;
+ VarsUi.State = 4;
+ }
+
+// Test for break;
+ switch (cUiReadButtons())
+ {
+ case BUTTON_EXIT :
+ {
+ VarsUi.State++;
+ }
+ break;
+
+ case BUTTON_LEFT :
+ {
+ VarsUi.Tmp = VarsUi.SelectedPort;
+ do
+ {
+ VarsUi.Tmp--;
+ if (VarsUi.Tmp <= MENU_PORT_EMPTY)
+ {
+ VarsUi.Tmp = MENU_PORT_INVALID - 1;
+ }
+ }
+ while (VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1] == MENU_SENSOR_EMPTY);
+ if ((VarsUi.Counter > 2) || (VarsUi.Tmp < VarsUi.SelectedPort))
+ {
+ VarsUi.SelectedPort = VarsUi.Tmp;
+ }
+ VarsUi.Update = TRUE;
+ }
+ break;
+
+ case BUTTON_RIGHT :
+ {
+ VarsUi.Tmp = VarsUi.SelectedPort;
+ do
+ {
+ VarsUi.Tmp++;
+ if (VarsUi.Tmp >= MENU_PORT_INVALID)
+ {
+ VarsUi.Tmp = MENU_PORT_1;
+ }
+ }
+ while (VarsUi.DatalogPort[VarsUi.Tmp - MENU_PORT_1] == MENU_SENSOR_EMPTY);
+ if ((VarsUi.Counter > 2) || (VarsUi.Tmp > VarsUi.SelectedPort))
+ {
+ VarsUi.SelectedPort = VarsUi.Tmp;
+ }
+ VarsUi.Update = TRUE;
+ }
+ break;
+
+ }
+ IOMapUi.Flags |= UI_RESET_SLEEP_TIMER;
+ }
+ break;
+
+ case 2 :
+ {
+// Close file
+ pMapLoader->pFunc(CROPDATAFILE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+
+// Clean up
+ pMapDisplay->pMenuText = VarsUi.MenuIconTextSave;
+ cUiReleaseSensors();
+
+ IOMapUi.Flags &= ~UI_BUSY;
+ IOMapUi.State = RIGHT_PRESSED;
+ VarsUi.State = 0;
+ }
+ break;
+
+ case 3 : // Display memory full text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_DL_ERROR_MEMORY_FULL_1,TXT_FB_DL_ERROR_MEMORY_FULL_2,DISPLAY_SHOW_ERROR_TIME))
+ {
+ cUiMenuPrevFile();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ case 4 : // Display memory full text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_DL_ERROR_MEMORY_FULL_1,TXT_FB_DL_ERROR_MEMORY_FULL_2,DISPLAY_SHOW_ERROR_TIME))
+ {
+ VarsUi.State = 2;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_SAVE : // Save datalog file
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.NVData.DatalogNumber++;
+ if (VarsUi.NVData.DatalogNumber > MAX_DATALOGS)
+ {
+ VarsUi.NVData.DatalogNumber = 1;
+ }
+ cUiNVWrite();
+ sprintf((char*)VarsUi.SelectedFilename,"%s%u.%s",(char*)UI_DATALOG_FILENAME,VarsUi.NVData.DatalogNumber,TXT_FILE_EXT[FILETYPE_DATALOG]);
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+// Rename TEMP_DATALOG_FILENAME to VarsUi.SelectedFilename(user filename)
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_DATALOG_FILENAME,(char*)TXT_FILE_EXT[FILETYPE_DATALOG]);
+ VarsUi.TmpHandle = pMapLoader->pFunc(RENAMEFILE,VarsUi.FilenameBuffer,VarsUi.SelectedFilename,&VarsUi.TmpLength);
+ VarsUi.State++;
+ }
+ break;
+
+ case 2 : // Display saved text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_DL_FILE_SAVED_INFO,0xFF,DISPLAY_SHOW_FILENAME_TIME))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default :
+ {
+ cUiMenuPrevFile();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_DELETE : // Delete datalog file
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+// Delete file if exist
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_DATALOG_FILENAME,(char*)TXT_FILE_EXT[FILETYPE_DATALOG]);
+ pMapLoader->pFunc(DELETE,VarsUi.FilenameBuffer,NULL,NULL);
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4) | TEXTLINE_BIT(TEXTLINE_5) | MENUICON_BITS | SPECIAL_BIT(MENUTEXT));
+ VarsUi.Timer = DISPLAY_SHOW_TIME;
+ VarsUi.State++;
+ }
+ break;
+
+ case 2 :
+ {
+ if (++VarsUi.Timer >= DISPLAY_SHOW_TIME)
+ {
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_3);
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default :
+ {
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_SELECT : // Save sensor
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_VIEW_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+
+ VarsUi.DatalogPort[VarsUi.SelectedPort - MENU_PORT_1] = VarsUi.SelectedSensor;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+ break;
+
+ default :
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ if ((Action > MENU_SENSOR_EMPTY) && (Action < MENU_SENSOR_INVALID))
+ {
+ VarsUi.SelectedSensor = Action;
+ }
+ if ((Action > MENU_PORT_EMPTY) && (Action < MENU_PORT_INVALID))
+ {
+ VarsUi.SelectedPort = Action;
+ if (VarsUi.DatalogPort[VarsUi.SelectedPort - MENU_PORT_1] != MENU_SENSOR_EMPTY)
+ {
+
+ // Port occupied
+ pMapDisplay->pTextLines[TEXTLINE_4] = cUiGetString(TXT_DATALOGGING_PORT_OCCUPIED);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4);
+ VarsUi.Timer = 0;
+ VarsUi.State++;
+ }
+ }
+ }
+ break;
+
+ default :
+ {
+ if ((++VarsUi.Timer >= DISPLAY_SHOW_TIME) || (BUTTON_NONE != cUiReadButtons()))
+ {
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_4);
+ cUiMenuPrev();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.State);
+}
+
+
+//******* cUiRunning **********************************************************
+
+void cUiRunning(UBYTE Action)
+{
+ switch (Action)
+ {
+ case MENU_INIT :
+ {
+ VarsUi.RunIconSave = pMapDisplay->pMenuIcons[MENUICON_CENTER];
+ VarsUi.RunBitmapPointer = 0;
+ VarsUi.RunTimer = 0;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(TOPLINE);
+ }
+ break;
+
+ case MENU_RUN :
+ {
+ if ((IOMapUi.Flags & UI_ENABLE_STATUS_UPDATE))
+ {
+ if (++VarsUi.RunTimer >= RUN_BITMAP_CHANGE_TIME)
+ {
+ VarsUi.RunTimer = 0;
+ if (++VarsUi.RunBitmapPointer >= Running->ItemsY )
+ {
+ VarsUi.RunBitmapPointer = 0;
+ }
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Running->Data[VarsUi.RunBitmapPointer * Running->ItemPixelsX * (Running->ItemPixelsY / 8)];
+ pMapDisplay->EraseMask |= MENUICON_BIT(MENUICON_CENTER);
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_CENTER);
+ }
+ }
+ }
+ break;
+
+ case MENU_UPDATE :
+ {
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Running->Data[VarsUi.RunBitmapPointer * Running->ItemPixelsX * (Running->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_CENTER);
+ }
+ break;
+
+ case MENU_EXIT :
+ {
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = VarsUi.RunIconSave;
+ pMapDisplay->UpdateMask = MENUICON_BITS | SPECIAL_BIT(MENUTEXT);
+ }
+ break;
+
+ }
+}
+
+//******* cUiOnBrickProgramming **********************************************
+
+UBYTE cUiOnBrickProgramming(UBYTE Action) // On brick programming
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Show motor / sensor text
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_ONBRICKPROGRAMMING_PLEASE_USE_PORT);
+ pMapDisplay->pTextLines[TEXTLINE_4] = cUiGetString(TXT_ONBRICKPROGRAMMING_1_TOUCH_SENSOR);
+ pMapDisplay->pTextLines[TEXTLINE_5] = cUiGetString(TXT_ONBRICKPROGRAMMING_2_SOUND_SENSOR);
+ pMapDisplay->pTextLines[TEXTLINE_6] = cUiGetString(TXT_ONBRICKPROGRAMMING_3_LIGHT_SENSOR);
+ pMapDisplay->pTextLines[TEXTLINE_7] = cUiGetString(TXT_ONBRICKPROGRAMMING_4_ULTRA_SONIC);
+ pMapDisplay->pTextLines[TEXTLINE_8] = cUiGetString(TXT_ONBRICKPROGRAMMING_BC_LR_MOTORS);
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4) | TEXTLINE_BIT(TEXTLINE_5) | TEXTLINE_BIT(TEXTLINE_6) | TEXTLINE_BIT(TEXTLINE_7) | TEXTLINE_BIT(TEXTLINE_8));
+ pMapDisplay->UpdateMask &= ~SPECIAL_BIT(FRAME_SELECT);
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4) | TEXTLINE_BIT(TEXTLINE_5) | TEXTLINE_BIT(TEXTLINE_6) | TEXTLINE_BIT(TEXTLINE_7) | TEXTLINE_BIT(TEXTLINE_8) | SPECIAL_BIT(TOPLINE));
+ pMapDisplay->TextLinesCenterFlags |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4) | TEXTLINE_BIT(TEXTLINE_5) | TEXTLINE_BIT(TEXTLINE_6) | TEXTLINE_BIT(TEXTLINE_7) | TEXTLINE_BIT(TEXTLINE_8));
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_TEXT : // Show empty program steps
+ {
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_LARGE);
+
+ VarsUi.pTmp = (UBYTE*)Cursor;
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < SIZE_OF_CURSOR) && (VarsUi.Tmp < (UBYTE)SIZEOF_DATA(Cursor));VarsUi.Tmp++)
+ {
+ VarsUi.CursorTmp[VarsUi.Tmp] = *VarsUi.pTmp;
+ VarsUi.pTmp++;
+ }
+
+ for (VarsUi.ProgramStepPointer = 0;VarsUi.ProgramStepPointer < ON_BRICK_PROGRAMSTEPS;VarsUi.ProgramStepPointer++)
+ {
+ VarsUi.ProgramSteps[VarsUi.ProgramStepPointer] = MENU_ACTION_EMPTY;
+ }
+ VarsUi.ProgramStepPointer = 0;
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_EXIT : // Delete one steps and exit at the end
+ {
+ if (VarsUi.ProgramStepPointer)
+ {
+ if (VarsUi.ProgramStepPointer < ON_BRICK_PROGRAMSTEPS)
+ {
+ VarsUi.ProgramSteps[VarsUi.ProgramStepPointer] = MENU_ACTION_EMPTY;
+ }
+ VarsUi.ProgramStepPointer--;
+ }
+ else
+ {
+ IOMapUi.State = NEXT_MENU;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_RUN : // Run program steps until end or user press exit button
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.pTmp = (UBYTE*)Cursor;
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < SIZE_OF_CURSOR) && (VarsUi.Tmp < (UBYTE)SIZEOF_DATA(Cursor));VarsUi.Tmp++)
+ {
+ VarsUi.CursorTmp[VarsUi.Tmp] = *VarsUi.pTmp;
+ VarsUi.pTmp++;
+ }
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)VarsUi.CursorTmp;
+ cUiRunning(MENU_INIT);
+ Action = MENU_DRAW;
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 : // If sound finished -> Init text and program pointer
+ {
+ if (SOUND_IDLE == pMapSound->State)
+ {
+ VarsUi.ProgramStepPointer = ON_BRICK_PROGRAMSTEPS;
+ VarsUi.MenuIconTextSave = pMapDisplay->pMenuText;
+ pMapDisplay->EraseMask |= SPECIAL_BIT(MENUTEXT);
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 : // load file to run
+ {
+ if (PROG_IDLE == pMapCmd->ProgStatus)
+ {
+ sprintf((char*)pMapCmd->FileName,"%s.%s",(char*)VM_PROGRAM_READER,(char*)TXT_SYS_EXT);
+ pMapCmd->ActivateFlag = TRUE;
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 3 : // Wait for end of file
+ {
+ if (PROG_RUNNING != pMapCmd->ProgStatus)
+ {
+ pMapCmd->ProgStatus = PROG_RESET;
+ VarsUi.State = 99;
+ VarsUi.ProgramStepPointer = ON_BRICK_PROGRAMSTEPS;
+ }
+ else
+ {
+ if (VarsUi.OBPTimer >= MIN_DISPLAY_UPDATE_TIME)
+ {
+ if (IOMapUi.OBPPointer != VarsUi.ProgramStepPointer)
+ {
+ VarsUi.ProgramStepPointer = IOMapUi.OBPPointer;
+ Action = MENU_DRAW;
+ }
+ }
+ }
+ }
+ break;
+
+ default : // Program stopped
+ {
+ pMapDisplay->pMenuText = VarsUi.MenuIconTextSave;
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(MENUTEXT);
+ Action = MENU_DRAW;
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ if (VarsUi.State)
+ {
+ cUiRunning(MENU_RUN);
+ }
+ else
+ {
+ cUiRunning(MENU_EXIT);
+ }
+ }
+ break;
+
+ case MENU_LEFT : // NA
+ {
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_RIGHT : // NA
+ {
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_UPDATE : // NA
+ {
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_SAVE : // Save NXT program
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ // Suggest default filename to user
+ strcpy((char*)VarsUi.UserString,(char*)DEFAULT_PROGRAM_NAME);
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+ if (!cUiGetUserString(1))
+ {
+ if (VarsUi.UserString[0])
+ {
+ sprintf((char*)VarsUi.SelectedFilename,"%s.%s",VarsUi.UserString,TXT_FILE_EXT[FILETYPE_NXT]);
+
+ // If tmp file exist -> ask for overwrite
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,(UBYTE*)VarsUi.SelectedFilename,VarsUi.FilenameBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State += 2;
+ }
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_OBP_FILE_EXIST_FAIL,TXT_FB_OBP_OVERWRITE_FAIL,0))
+ {
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ case 3 :
+ {
+ // Rename TEMP_PROGRAM_FILENAME to VarsUi.SelectedFilename(user filename)
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_PROGRAM_FILENAME,(char*)TXT_TMP_EXT);
+ VarsUi.TmpHandle = pMapLoader->pFunc(RENAMEFILE,VarsUi.FilenameBuffer,VarsUi.SelectedFilename,&VarsUi.TmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ VarsUi.State++;
+ }
+ break;
+
+ case 4 : // Display saved text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_OBP_FILE_SAVED_INFO,0,DISPLAY_SHOW_TIME))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default :
+ {
+ cUiMenuPrevFile();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_OVERWRITE : // Over write existing file
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ // Delete VarsUi.SelectedFilename(user filename)
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,(UBYTE*)VarsUi.SelectedFilename,VarsUi.FilenameBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.SelectedFilename,NULL,NULL);
+ }
+
+ // Rename TEMP_PROGRAM_FILENAME to VarsUi.SelectedFilename(user filename)
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_PROGRAM_FILENAME,(char*)TXT_TMP_EXT);
+ VarsUi.TmpHandle = pMapLoader->pFunc(RENAMEFILE,VarsUi.FilenameBuffer,VarsUi.SelectedFilename,&VarsUi.TmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ VarsUi.State++;
+ }
+ break;
+
+ default : // Display saved text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_OBP_FILE_SAVED_INFO,0,DISPLAY_SHOW_TIME))
+ {
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+
+ }
+ break;
+
+ default : // Insert selected action/waitfor in program and save if finished
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.ProgramSteps[VarsUi.ProgramStepPointer] = Action;
+ if (VarsUi.ProgramStepPointer < ON_BRICK_PROGRAMSTEPS)
+ {
+ VarsUi.ProgramStepPointer++;
+ }
+ if (VarsUi.ProgramStepPointer == ON_BRICK_PROGRAMSTEPS)
+ {
+ // If tmp file exist -> delete it
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_PROGRAM_FILENAME,(char*)TXT_TMP_EXT);
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FilenameBuffer,VarsUi.SearchFilenameBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.FilenameBuffer,NULL,NULL);
+ }
+
+ // Save program as tmp file
+ VarsUi.TmpLength = FILEHEADER_LENGTH + ON_BRICK_PROGRAMSTEPS;
+ VarsUi.TmpHandle = pMapLoader->pFunc(OPENWRITE,VarsUi.FilenameBuffer,NULL,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ VarsUi.FileHeader[0] = (UBYTE)(FILEFORMAT_PROGRAM >> 8);
+ VarsUi.FileHeader[1] = (UBYTE)(FILEFORMAT_PROGRAM);
+ VarsUi.FileHeader[2] = (UBYTE)(ON_BRICK_PROGRAMSTEPS >> 8);
+ VarsUi.FileHeader[3] = (UBYTE)(ON_BRICK_PROGRAMSTEPS);
+ VarsUi.FileHeader[4] = (UBYTE)(ON_BRICK_PROGRAMSTEPS);
+ VarsUi.FileHeader[5] = (UBYTE)0;
+ VarsUi.FileHeader[6] = (UBYTE)0;
+ VarsUi.FileHeader[7] = (UBYTE)0;
+ VarsUi.TmpLength = FILEHEADER_LENGTH;
+ pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)VarsUi.FileHeader,&VarsUi.TmpLength);
+ VarsUi.TmpLength = ON_BRICK_PROGRAMSTEPS;
+ pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)VarsUi.ProgramSteps,&VarsUi.TmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ default : // Display memory error text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_OBP_MEMORY_FULL_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ cUiMenuPrevFile();
+ IOMapUi.State = NEXT_MENU;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+
+ // Update display screen
+ if (Action == MENU_DRAW)
+ {
+ VarsUi.OBPTimer = 0;
+
+ for (VarsUi.Pointer = 0;VarsUi.Pointer < ON_BRICK_PROGRAMSTEPS;VarsUi.Pointer++)
+ {
+ VarsUi.Tmp = VarsUi.ProgramSteps[VarsUi.Pointer];
+ if ((VarsUi.Tmp >= MENU_ACTION_EMPTY) && (VarsUi.Tmp < MENU_ACTION_INVALID))
+ {
+ VarsUi.Tmp -= MENU_ACTION_EMPTY;
+ pMapDisplay->StepIcons[VarsUi.Pointer] = VarsUi.Tmp + 1;
+ }
+ if ((VarsUi.Tmp >= MENU_WAIT_EMPTY) && (VarsUi.Tmp < MENU_WAIT_INVALID))
+ {
+ VarsUi.Tmp -= MENU_WAIT_EMPTY;
+ pMapDisplay->StepIcons[VarsUi.Pointer] = VarsUi.Tmp + 1 + 16;
+ }
+ if (VarsUi.Tmp == MENU_LOOP)
+ {
+ pMapDisplay->StepIcons[VarsUi.Pointer] = 31;
+ }
+ if (VarsUi.Tmp == MENU_STOP)
+ {
+ pMapDisplay->StepIcons[VarsUi.Pointer] = 32;
+ }
+ pMapDisplay->UpdateMask |= STEPICON_BIT(STEPICON_1 + VarsUi.Pointer);
+ }
+
+ // and cursor
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)VarsUi.CursorTmp;
+ if (VarsUi.ProgramStepPointer < ON_BRICK_PROGRAMSTEPS)
+ {
+ VarsUi.CursorTmp[4] = 13 + (VarsUi.ProgramStepPointer * 17);
+ VarsUi.CursorTmp[5] = 24;
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_1);
+ }
+ if (PROG_RUNNING != pMapCmd->ProgStatus)
+ {
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_2));
+ }
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->UpdateMask |= (SPECIAL_BIT(STEPLINE) | SPECIAL_BIT(TOPLINE));
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiFileRun **********************************************************
+
+UBYTE cUiFindFileType(UBYTE *Filename) // Find file type number
+{
+ UBYTE Ext[FILENAME_LENGTH + 1];
+ UBYTE Result;
+ UBYTE Tmp1;
+ UBYTE Tmp2;
+
+ Result = FILETYPE_ALL;
+
+ Tmp1 = 0;
+ while ((Filename[Tmp1]) && (Tmp1 < FILENAME_LENGTH)) // Search forward for termination
+ {
+ Tmp1++;
+ }
+
+ while ((Tmp1) && (Filename[Tmp1] != '.')) // Search backward for "."
+ {
+ Tmp1--;
+ }
+
+ if (Filename[Tmp1] == '.') // If "."
+ {
+ Tmp1++;
+ Tmp2 = 0;
+
+ while ((Filename[Tmp1]) && (Tmp1 < FILENAME_LENGTH)) // Convert to upper to Ext
+ {
+ Ext[Tmp2] = tolower(Filename[Tmp1]);
+ Tmp1++;
+ Tmp2++;
+ }
+ Ext[Tmp2] = 0; // Inser termination
+
+ // Calculate type
+ for (Tmp1 = FILETYPE_ALL;(Tmp1 < FILETYPES) && (Result == FILETYPE_ALL);Tmp1++)
+ {
+ if (strcmp((char*)TXT_FILE_EXT[Tmp1],(char*)Ext) == 0)
+ {
+ Result = Tmp1;
+ }
+ }
+ }
+
+ return (Result);
+}
+
+
+#define FILERUN_FILENAMELINE TEXTLINE_4
+#define FILERUN_TEXTLINE TEXTLINE_5
+
+UBYTE cUiFileRun(UBYTE Action) // Run selected file
+{
+ switch (Action)
+ {
+
+ case MENU_INIT :
+ {
+ VarsUi.Tmp = 0;
+ while ((VarsUi.SelectedFilename[VarsUi.Tmp]) && (VarsUi.Tmp < FILENAME_LENGTH)) // Search forward for termination
+ {
+ VarsUi.Tmp++;
+ }
+
+ while ((VarsUi.Tmp) && (VarsUi.SelectedFilename[VarsUi.Tmp] != '.')) // Search backward for "."
+ {
+ VarsUi.Tmp--;
+ }
+
+ if (VarsUi.Tmp > DISPLAYLINE_LENGTH)
+ {
+ VarsUi.Tmp = DISPLAYLINE_LENGTH;
+ }
+
+ VarsUi.DisplayBuffer[VarsUi.Tmp] = 0;
+
+ while (VarsUi.Tmp) // Copy only name not ext
+ {
+ VarsUi.Tmp--;
+ VarsUi.DisplayBuffer[VarsUi.Tmp] = VarsUi.SelectedFilename[VarsUi.Tmp];
+ }
+
+ pMapDisplay->pTextLines[FILERUN_FILENAMELINE] = (UBYTE*)VarsUi.DisplayBuffer;
+ pMapDisplay->TextLinesCenterFlags = TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ pMapDisplay->UpdateMask = TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ }
+ break;
+
+ case MENU_RUN :
+ {
+ if (VarsUi.Timer < DISPLAY_SHOW_TIME)
+ {
+ VarsUi.Timer++;
+ }
+
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ IOMapUi.Flags |= UI_BUSY;
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 : // Set state from extention when sound is ready
+ {
+ if (SOUND_IDLE == pMapSound->State)
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_RUNNING);
+ pMapDisplay->UpdateMask = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ pMapDisplay->TextLinesCenterFlags = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ cUiRunning(MENU_INIT);
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ if ((!pMapDisplay->EraseMask) && (!pMapDisplay->UpdateMask))
+ {
+ VarsUi.State = 10 * cUiFindFileType(VarsUi.SelectedFilename);
+ if (VarsUi.State == (FILETYPE_TRYME * 10))
+ {
+ VarsUi.State = FILETYPE_LMS * 10;
+ }
+ }
+ }
+ break;
+
+ case (FILETYPE_SOUND * 10 + 0) : // Start sound file (*.snd, *.rso) Wait for sound idle
+ {
+ strcpy((char*)pMapSound->SoundFilename,(char*)VarsUi.SelectedFilename);
+ pMapSound->Volume = IOMapUi.Volume;
+ pMapSound->Mode = SOUND_ONCE;
+ pMapSound->Flags |= SOUND_UPDATE;
+ VarsUi.State++;
+ }
+ break;
+
+ case (FILETYPE_SOUND * 10 + 1) : // Wait for stop or user break
+ {
+ cUiRunning(MENU_RUN);
+
+ if (SOUND_IDLE == pMapSound->State)
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_ENDED);
+ VarsUi.State = 99;
+ }
+ if (BUTTON_EXIT == cUiReadButtons())
+ {
+ pMapSound->Flags &= ~SOUND_UPDATE;
+ pMapSound->State = SOUND_STOP;
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_ABORTED);
+ VarsUi.State = 99;
+ }
+ }
+ break;
+
+ case (FILETYPE_LMS * 10 + 0) : // Start LMS file (*.rxe)
+ {
+ if ((!pMapDisplay->EraseMask) && (pMapCmd->ProgStatus == PROG_IDLE) && (!pMapButton->State[BTN4]))
+ {
+ strcpy((char*)pMapCmd->FileName,(char*)VarsUi.SelectedFilename);
+ pMapCmd->ActivateFlag = TRUE;
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case (FILETYPE_LMS * 10 + 1) : // Wait for program stop or user break
+ {
+ cUiRunning(MENU_RUN);
+
+ if ((IOMapUi.Flags & UI_REDRAW_STATUS) && (IOMapUi.Flags & UI_ENABLE_STATUS_UPDATE))
+ {
+ pMapDisplay->pTextLines[FILERUN_FILENAMELINE] = (UBYTE*)VarsUi.DisplayBuffer;
+ pMapDisplay->TextLinesCenterFlags = TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ pMapDisplay->UpdateMask = TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_RUNNING);
+ pMapDisplay->UpdateMask = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ pMapDisplay->TextLinesCenterFlags = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ }
+
+ switch (pMapCmd->ProgStatus)
+ {
+ case PROG_RUNNING :
+ {
+ }
+ break;
+
+ case PROG_OK :
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_ENDED);
+ VarsUi.State = 99;
+ }
+ break;
+
+ case PROG_ABORT :
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_ABORTED);
+ VarsUi.State = 99;
+ }
+ break;
+
+ default :
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_FILE_ERROR);
+ VarsUi.State = 99;
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case (FILETYPE_NXT * 10 + 0) :// Start Program file (*.prg)
+ {
+ VarsUi.TmpHandle = pMapLoader->pFunc(OPENREAD,VarsUi.SelectedFilename,NULL,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ VarsUi.TmpLength = FILEHEADER_LENGTH;
+ pMapLoader->pFunc(READ,(UBYTE*)&VarsUi.TmpHandle,VarsUi.FileHeader,&VarsUi.TmpLength);
+ VarsUi.TmpLength = ON_BRICK_PROGRAMSTEPS;
+ pMapLoader->pFunc(READ,(UBYTE*)&VarsUi.TmpHandle,VarsUi.ProgramSteps,&VarsUi.TmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ }
+ if ((ON_BRICK_PROGRAMSTEPS == VarsUi.TmpLength) && (VarsUi.FileHeader[0] == (UBYTE)(FILEFORMAT_PROGRAM >> 8)) && (VarsUi.FileHeader[1] == (UBYTE)(FILEFORMAT_PROGRAM)))
+ {
+ // If tmp file exist -> delete it
+ sprintf((char*)VarsUi.FilenameBuffer,"%s.%s",(char*)TEMP_PROGRAM_FILENAME,(char*)TXT_TMP_EXT);
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FilenameBuffer,VarsUi.SearchFilenameBuffer,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.FilenameBuffer,NULL,NULL);
+ }
+
+ // Save program as tmp file
+ VarsUi.TmpLength = FILEHEADER_LENGTH + ON_BRICK_PROGRAMSTEPS;
+ VarsUi.TmpHandle = pMapLoader->pFunc(OPENWRITE,VarsUi.FilenameBuffer,NULL,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ VarsUi.TmpLength = FILEHEADER_LENGTH;
+ pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)VarsUi.FileHeader,&VarsUi.TmpLength);
+ VarsUi.TmpLength = ON_BRICK_PROGRAMSTEPS;
+ pMapLoader->pFunc(WRITE,(UBYTE*)&VarsUi.TmpHandle,(UBYTE*)VarsUi.ProgramSteps,&VarsUi.TmpLength);
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ }
+
+ pMapDisplay->UpdateMask &= ~TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ VarsUi.State++;
+ }
+ else
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_FILE_ERROR);
+ VarsUi.State = 99;
+ }
+ VarsUi.GUSState = 0;
+ }
+ break;
+
+ case (FILETYPE_NXT * 10 + 1) : // Wait for program stop or user break
+ {
+ VarsUi.State = VarsUi.GUSState;
+ cUiOnBrickProgramming(MENU_RUN);
+ VarsUi.GUSState = VarsUi.State;
+ if (VarsUi.State)
+ {
+ VarsUi.State = (FILETYPE_NXT * 10 + 1);
+ }
+ else
+ {
+ pMapDisplay->pTextLines[FILERUN_TEXTLINE] = cUiGetString(TXT_FILERUN_ENDED);
+ VarsUi.State = 99;
+ }
+ }
+ break;
+
+ case 99 : // Wait for display show time or user action
+ {
+ pMapDisplay->EraseMask = SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->UpdateMask = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ pMapDisplay->TextLinesCenterFlags = (TEXTLINE_BIT(FILERUN_TEXTLINE) | TEXTLINE_BIT(FILERUN_FILENAMELINE));
+ IOMapUi.Flags |= UI_REDRAW_STATUS | UI_ENABLE_STATUS_UPDATE;
+ cUiRunning(MENU_UPDATE);
+ VarsUi.Timer = 0;
+ VarsUi.State++;
+ }
+ break;
+
+ default :
+ {
+ if ((++VarsUi.Timer >= DISPLAY_SHOW_TIME) || (BUTTON_NONE != cUiReadButtons()))
+ {
+ if (pMapCmd->ProgStatus != PROG_IDLE)
+ pMapCmd->ProgStatus = PROG_RESET;
+ pMapDisplay->UpdateMask = 0;
+ pMapDisplay->TextLinesCenterFlags = 0;
+ cUiRunning(MENU_EXIT);
+ pMapDisplay->EraseMask = TEXTLINE_BIT(FILERUN_TEXTLINE);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(FILERUN_FILENAMELINE);
+ IOMapUi.Flags &= ~UI_BUSY;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiFileDelete *******************************************************
+
+UBYTE cUiFileDelete(UBYTE Action)
+{
+ if (MENU_INIT == Action)
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+ if (SOUND_IDLE == pMapSound->State)
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ pMapLoader->pFunc(DELETE,VarsUi.SelectedFilename,NULL,NULL);
+ VarsUi.State++;
+ }
+ break;
+
+ default : // Display deleted text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_FD_FILE_DELETED_INFO,0,DISPLAY_SHOW_TIME))
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+ }
+
+ return (VarsUi.State);
+}
+
+
+//******* cUiView ************************************************************
+
+UBYTE cUiView(UBYTE Action) // MENU_INIT
+{
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ switch (Action)
+ {
+ case MENU_INIT : // Init
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_VIEW_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+// Init ports
+ for (VarsUi.Tmp = 0;VarsUi.Tmp < DATALOGPORTS;VarsUi.Tmp++)
+ {
+ VarsUi.DatalogPort[VarsUi.Tmp] = MENU_SENSOR_EMPTY;
+ }
+ }
+ break;
+
+ default :
+ {
+ if ((Action > MENU_SENSOR_EMPTY) && (Action < MENU_SENSOR_INVALID))
+ {
+ VarsUi.SelectedSensor = Action;
+ }
+ if ((Action >= MENU_PORT_1) && (Action <= MENU_PORT_C))
+ {
+ VarsUi.SelectedPort = Action;
+
+ VarsUi.DatalogPort[VarsUi.SelectedPort - MENU_PORT_1] = VarsUi.SelectedSensor;
+
+ IOMapUi.Flags |= UI_BUSY;
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)Display;
+ pMapDisplay->UpdateMask = BITMAP_BIT(BITMAP_1);
+ IOMapUi.Flags |= UI_REDRAW_STATUS;
+ VarsUi.ReadoutTimer = 0;;
+ VarsUi.State++;
+
+ VarsUi.SensorReset = TRUE;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case 1 :
+ {
+ VarsUi.ReadoutTimer++;
+ cUiUpdateSensor(1);
+ if (VarsUi.ReadoutTimer >= DISPLAY_VIEW_UPDATE)
+ {
+ VarsUi.ReadoutTimer = 0;
+ cUiPrintSensorInDisplayBuffer(VarsUi.SelectedPort);
+ pMapDisplay->pTextLines[TEXTLINE_4] = VarsUi.DisplayBuffer;
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4);
+ }
+
+ VarsUi.Tmp = cUiReadButtons();
+ if (VarsUi.Tmp == BUTTON_EXIT)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_VIEW_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask &= ~TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->EraseMask |= SCREEN_BIT(SCREEN_SMALL);
+ VarsUi.State++;
+ }
+ if (VarsUi.Tmp == BUTTON_ENTER)
+ {
+ VarsUi.SensorReset = TRUE;
+ }
+ }
+ break;
+
+ default :
+ {
+ cUiReleaseSensors();
+ IOMapUi.Flags &= ~UI_BUSY;
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+ break;
+
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiBtOn ************************************************************
+
+UBYTE cUiBtOn(UBYTE Action)
+{
+ switch (Action)
+ {
+ case MENU_ON :
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Turn BT on
+ {
+ VarsUi.BTCommand = (UBYTE)BTON;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ break;
+
+ case 1 : // Display turning on text
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_TURNING_ON_WAIT,0,0))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 : // Check result
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ }
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_TURNING_ON_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_OFF :
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Turn BT off
+ {
+ VarsUi.BTCommand = (UBYTE)BTOFF;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ break;
+
+ case 1 : // Display turning off text
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_TURNING_OFF_WAIT,0,0))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 : // Check result
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ }
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_TURNING_OFF_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+ if (Action == MENU_EXIT)
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiBtVisiability ***************************************************
+
+UBYTE cUiBtVisiability(UBYTE Action) // Visibility on/off
+{
+ switch (Action)
+ {
+ case MENU_ON : // Set visible
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.BTCommand = (UBYTE)VISIBILITY;
+ VarsUi.BTPar1 = (UBYTE)1;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ default :
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_OFF : // Set invisible
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.BTCommand = (UBYTE)VISIBILITY;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ default :
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+ if (Action == MENU_EXIT)
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiBtSearch ********************************************************
+
+UBYTE cUiBtSearch(UBYTE Action) // Search for devices
+{
+ if (Action == MENU_INIT) // Init
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Show three menu icons
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = pMapDisplay->pMenuIcons[MENUICON_CENTER];
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = pMapDisplay->pMenuIcons[MENUICON_CENTER];
+ pMapDisplay->UpdateMask |= MENUICON_BITS;
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 : // Display wait text and start search
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_SEARCHING_WAIT,0,0))
+ {
+ VarsUi.BTCommand = (UBYTE)SEARCH;
+ VarsUi.BTPar1 = (UBYTE)1;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.DisplayBuffer[0] = 0;
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(MENUTEXT);
+ VarsUi.NoOfNames = 0;
+ VarsUi.NoOfDevices = 0;
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ case 2 : // Wait for search finished
+ {
+ if (VarsUi.NoOfNames != pMapComm->BtDeviceNameCnt)
+ {
+ VarsUi.NoOfNames = pMapComm->BtDeviceNameCnt;
+
+ if ((VarsUi.NoOfNames) && (VarsUi.NoOfNames <= DISPLAYLINE_LENGTH))
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,"%.*s",VarsUi.NoOfNames,"****************");
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(MENUTEXT);
+ }
+ }
+ if (VarsUi.NoOfDevices != pMapComm->BtDeviceCnt)
+ {
+ VarsUi.NoOfDevices = pMapComm->BtDeviceCnt;
+
+ if ((VarsUi.NoOfDevices) && (VarsUi.NoOfDevices <= DISPLAYLINE_LENGTH))
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,"%.*s",VarsUi.NoOfDevices,"????????????????");
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+ pMapDisplay->UpdateMask |= SPECIAL_BIT(MENUTEXT);
+ }
+ }
+
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ cUiBTCommand(UI_BT_GET_DEVICES,0,&VarsUi.Devices,NULL);
+ if (VarsUi.Devices)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+
+ if (cUiReadButtons() == BUTTON_EXIT)
+ {
+ VarsUi.BTCommand = (UBYTE)STOPSEARCH;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult));
+ VarsUi.State = 4;
+ }
+ }
+ break;
+
+ case 3 : // Auto enter to next menu
+ {
+ IOMapUi.State = ENTER_PRESSED;
+ VarsUi.State = 0;
+ }
+ break;
+
+ case 4 : // Display info text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_BT_SEARCH_ABORTED_INFO,0,DISPLAY_SHOW_TIME))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 5 : // Wait for abort
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ cUiBTCommand(UI_BT_GET_DEVICES,0,&VarsUi.Devices,NULL);
+ if (VarsUi.Devices)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ case 6 : // Auto enter to next menu
+ {
+ IOMapUi.State = ENTER_PRESSED;
+ VarsUi.State = 0;
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_SEARCHING_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+ }
+ break;
+
+ }
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiBtDeviceList ****************************************************
+
+UBYTE cUiBtDeviceList(UBYTE Action) // Show devices
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Init "Search" list
+ {
+ VarsUi.SelectedDevice = 0;
+ VarsUi.DevicesKnown = 0;
+ cUiBTCommand(UI_BT_GET_DEVICES,VarsUi.DevicesKnown,&VarsUi.Devices,NULL);
+ if (VarsUi.Devices)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_BTDEVICELIST_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ VarsUi.MenuIconTextSave = pMapDisplay->pMenuText;
+ VarsUi.DeviceCenter = 1;
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case MENU_INIT_ALTERNATIVE : // Init only "My contacts"
+ {
+ VarsUi.SelectedDevice = 0;
+ VarsUi.DevicesKnown = 1;
+ cUiBTCommand(UI_BT_GET_DEVICES,VarsUi.DevicesKnown,&VarsUi.Devices,NULL);
+ if (VarsUi.Devices)
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_BTDEVICELIST_SELECT);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ VarsUi.MenuIconTextSave = pMapDisplay->pMenuText;
+ VarsUi.DeviceCenter = 1;
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case MENU_LEFT : // Left button
+ {
+ cUiListLeft(VarsUi.Devices,&VarsUi.DeviceCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_RIGHT : // Right button
+ {
+ cUiListRight(VarsUi.Devices,&VarsUi.DeviceCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_SELECT : // Select for connection
+ {
+ VarsUi.SelectedDevice = VarsUi.DeviceCenter;
+ pMapDisplay->pMenuText = VarsUi.MenuIconTextSave;
+ IOMapUi.State = NEXT_MENU;
+ }
+ break;
+
+ case MENU_DELETE : // Remove device from "My contacts"
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ if (VarsUi.SelectedDevice)
+ {
+ if (cUiBTGetDeviceIndex(VarsUi.DevicesKnown,VarsUi.SelectedDevice - 1,&VarsUi.BTIndex))
+ {
+ VarsUi.BTCommand = (UBYTE)REMOVEDEVICE;
+ VarsUi.BTPar1 = (UBYTE)VarsUi.BTIndex;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ VarsUi.SelectedDevice = 0;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case 1 :
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_REMOVE_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+
+ if (Action == MENU_DRAW)
+ {
+ cUiListCalc(VarsUi.Devices,&VarsUi.DeviceCenter,&VarsUi.DeviceLeft,&VarsUi.DeviceRight);
+
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = NULL;
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = NULL;
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = NULL;
+
+ if (VarsUi.DeviceLeft)
+ {
+ VarsUi.Tmp = VarsUi.DeviceLeft - 1;
+ cUiBTCommand(UI_BT_GET_DEVICE_TYPE,VarsUi.DevicesKnown,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_LEFT);
+ }
+ if (VarsUi.DeviceCenter)
+ {
+ VarsUi.Tmp = VarsUi.DeviceCenter - 1;
+ cUiBTCommand(UI_BT_GET_DEVICE_TYPE,VarsUi.DevicesKnown,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_CENTER);
+ }
+ if (VarsUi.DeviceRight)
+ {
+ VarsUi.Tmp = VarsUi.DeviceRight - 1;
+ cUiBTCommand(UI_BT_GET_DEVICE_TYPE,VarsUi.DevicesKnown,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ pMapDisplay->UpdateMask |= MENUICON_BIT(MENUICON_RIGHT);
+ }
+
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_5);
+
+ VarsUi.Tmp = VarsUi.DeviceCenter - 1;
+ cUiBTCommand(UI_BT_GET_DEVICE_NAME,VarsUi.DevicesKnown,&VarsUi.Tmp,VarsUi.DisplayBuffer);
+
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+ pMapDisplay->EraseMask |= MENUICON_BITS;
+ pMapDisplay->UpdateMask |= (SPECIAL_BIT(FRAME_SELECT) | SPECIAL_BIT(MENUTEXT));
+ }
+ if (Action == MENU_EXIT)
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+ return (VarsUi.State);
+}
+
+
+//******* cUiBtConnectList ***************************************************
+
+UBYTE cUiBtConnectList(UBYTE Action) // Show connections and maybe disconnect
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Init
+ {
+ VarsUi.Slots = SIZE_OF_BT_CONNECT_TABLE;
+ VarsUi.SlotCenter = 2;
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_LEFT : // Left button
+ {
+ cUiListLeft(VarsUi.Slots,&VarsUi.SlotCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_RIGHT : // Right button
+ {
+ cUiListRight(VarsUi.Slots,&VarsUi.SlotCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_UPDATE : // Check connection valid
+ {
+ VarsUi.Tmp = VarsUi.SlotCenter - 1;
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) != UI_BT_SUCCES)
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case MENU_DISCONNECT : // Disconnect
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ VarsUi.SelectedSlot = VarsUi.SlotCenter - 1;
+ VarsUi.BTCommand = (UBYTE)DISCONNECT;
+ VarsUi.BTPar1 = (UBYTE)VarsUi.SelectedSlot;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ break;
+
+ case 1 :
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_DISCONNECT_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+ if (Action == MENU_DRAW)
+ {
+ cUiListCalc(VarsUi.Slots,&VarsUi.SlotCenter,&VarsUi.SlotLeft,&VarsUi.SlotRight);
+
+ pMapDisplay->pBitmaps[BITMAP_2] = (BMPMAP*)VarsUi.PortBitmapLeft;
+ pMapDisplay->pBitmaps[BITMAP_3] = (BMPMAP*)VarsUi.PortBitmapCenter;
+ pMapDisplay->pBitmaps[BITMAP_4] = (BMPMAP*)VarsUi.PortBitmapRight;
+
+ VarsUi.Tmp = VarsUi.SlotLeft - 1;
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapLeft,MENUICON_LEFT,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_2);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+
+ VarsUi.Tmp = VarsUi.SlotCenter - 1;
+ cUiBTCommand(UI_BT_GET_CONNECTION_NAME,NULL,&VarsUi.Tmp,VarsUi.DisplayBuffer);
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_5);
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapCenter,MENUICON_CENTER,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_3);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+ VarsUi.Tmp = VarsUi.SlotRight - 1;
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapRight,MENUICON_RIGHT,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_4);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+ pMapDisplay->EraseMask &= ~SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->EraseMask |= MENUICON_BITS;
+ pMapDisplay->UpdateMask |= (MENUICON_BITS | SPECIAL_BIT(FRAME_SELECT) | SPECIAL_BIT(MENUTEXT));
+ }
+ if (Action == MENU_EXIT)
+ {
+ VarsUi.State = 0;
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+
+ return (VarsUi.State);
+}
+
+
+UBYTE cUiBtConnect(UBYTE Action) // Select connection no and insert device
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Init
+ {
+ VarsUi.Slots = SIZE_OF_BT_CONNECT_TABLE - 1;
+ VarsUi.SlotCenter = 1;
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_LEFT : // Left button
+ {
+ cUiListLeft(VarsUi.Slots,&VarsUi.SlotCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_RIGHT : // Right button
+ {
+ cUiListRight(VarsUi.Slots,&VarsUi.SlotCenter);
+ Action = MENU_DRAW;
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_CONNECT : // Insert device
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Check selected device
+ {
+ VarsUi.SelectedSlot = (UBYTE)VarsUi.SlotCenter;
+ if (VarsUi.SelectedDevice)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case 1 : // Display wait text
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_CONNECTING_WAIT,0,0))
+ {
+ if (cUiBTGetDeviceIndex(VarsUi.DevicesKnown,VarsUi.SelectedDevice - 1,&VarsUi.BTIndex))
+ {
+ VarsUi.BTCommand = (UBYTE)CONNECT;
+ VarsUi.BTPar1 = (UBYTE)VarsUi.BTIndex;
+ VarsUi.BTPar2 = (UBYTE)VarsUi.SelectedSlot;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ else
+ {
+ VarsUi.State = 99;
+ }
+ }
+ }
+ break;
+
+ case 2 : // Wait for result
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ if (VarsUi.BTResult == REQPIN)
+ {
+ sprintf((char*)pMapSound->SoundFilename,"%s.%s",(char*)UI_ATTENTION_SOUND,(char*)TXT_FILE_EXT[FILETYPE_SOUND]);
+ pMapSound->Volume = IOMapUi.Volume;
+ pMapSound->Mode = SOUND_ONCE;
+ pMapSound->Flags |= SOUND_UPDATE;
+ strcpy((char*)VarsUi.UserString,(char*)DEFAULT_PIN_CODE);
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 6;
+ }
+ }
+ }
+ }
+ break;
+
+ case 3 : // Get pincode and send
+ {
+ if (!cUiGetUserString(0))
+ {
+ if (VarsUi.UserString[0] == 0)
+ {
+ sprintf((char*)VarsUi.UserString,"%08lX",VarsUi.CRPasskey);
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ pMapComm->pFunc2(VarsUi.UserString);
+ }
+ }
+ break;
+
+ case 4 : // Display wait text
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_CONNECTING_WAIT,0,0))
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 5 : // Wait for result
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ Action = MENU_EXIT;
+ }
+ else
+ {
+ VarsUi.State = 6;
+ }
+ }
+ }
+ break;
+
+ case 6 : // Display busy text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_CONNECT_BUSY_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ default : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_CONNECTING_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ case MENU_SEND :
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Check connection
+ {
+ VarsUi.SelectedSlot = (UBYTE)VarsUi.SlotCenter;
+ if (VarsUi.SelectedFilename[0] && (cUiBTCommand(UI_BT_GET_CONNECTION_NAME,NULL,&VarsUi.SelectedSlot,NULL) == UI_BT_SUCCES))
+ {
+ VarsUi.State += 2;
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 1 : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_SENDING_NO_CONN_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case 2 : // Display wait text and send file
+ {
+ if (!cUiFeedback((BMPMAP*)Wait,TXT_FB_BT_SENDING_WAIT,0,0))
+ {
+ VarsUi.BTCommand = (UBYTE)SENDFILE;
+ VarsUi.BTPar1 = (UBYTE)VarsUi.SelectedSlot;
+ VarsUi.BTPar2 = (UBYTE)0;
+ if (pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,VarsUi.SelectedFilename,&(VarsUi.BTResult)) == SUCCESS)
+ {
+ VarsUi.Timer = 0;
+ VarsUi.State++;
+ }
+ else
+ {
+ VarsUi.State = 4;
+ }
+ }
+ }
+ break;
+
+ case 3 : // Wait for result
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ if (VarsUi.BTResult == SUCCESS)
+ {
+ VarsUi.State += 2;
+ }
+ else
+ {
+ VarsUi.State++;
+ }
+ }
+ VarsUi.Timer++;
+ }
+ break;
+
+ case 4 : // Display fail text
+ {
+ if (!cUiFeedback((BMPMAP*)Fail,TXT_FB_BT_SENDING_FAIL,0,DISPLAY_SHOW_ERROR_TIME))
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ case 5 : // Wait min. "DISPLAY_SHOW_TIME" to show "TXT_FB_BT_SENDING_WAIT"
+ {
+ if (++VarsUi.Timer >= DISPLAY_SHOW_TIME)
+ {
+ Action = MENU_EXIT;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+ if (Action == MENU_DRAW) // Update display
+ {
+ cUiListCalc(VarsUi.Slots,&VarsUi.SlotCenter,&VarsUi.SlotLeft,&VarsUi.SlotRight);
+
+ pMapDisplay->pBitmaps[BITMAP_2] = (BMPMAP*)VarsUi.PortBitmapLeft;
+ pMapDisplay->pBitmaps[BITMAP_3] = (BMPMAP*)VarsUi.PortBitmapCenter;
+ pMapDisplay->pBitmaps[BITMAP_4] = (BMPMAP*)VarsUi.PortBitmapRight;
+
+ VarsUi.Tmp = VarsUi.SlotLeft;
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapLeft,MENUICON_LEFT,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_2);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_LEFT] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+
+ VarsUi.Tmp = VarsUi.SlotCenter;
+ cUiBTCommand(UI_BT_GET_CONNECTION_NAME,NULL,&VarsUi.Tmp,VarsUi.DisplayBuffer);
+ pMapDisplay->EraseMask |= TEXTLINE_BIT(TEXTLINE_5);
+ pMapDisplay->pMenuText = VarsUi.DisplayBuffer;
+
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapCenter,MENUICON_CENTER,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_3);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_CENTER] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+ VarsUi.Tmp = VarsUi.SlotRight;
+ if (cUiBTCommand(UI_BT_GET_CONNECTION_VALID,NULL,&VarsUi.Tmp,NULL) == UI_BT_SUCCES)
+ {
+ cUiBTCommand(UI_BT_GET_CONNECTION_TYPE,NULL,&VarsUi.Tmp,&VarsUi.DeviceType);
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Devices->Data[VarsUi.DeviceType * Devices->ItemPixelsX * (Devices->ItemPixelsY / 8)];
+ cUiDrawPortNo(VarsUi.PortBitmapRight,MENUICON_RIGHT,VarsUi.Tmp);
+ pMapDisplay->UpdateMask |= BITMAP_BIT(BITMAP_4);
+ }
+ else
+ {
+ pMapDisplay->pMenuIcons[MENUICON_RIGHT] = (UBYTE*)&Connections->Data[VarsUi.Tmp * Connections->ItemPixelsX * (Connections->ItemPixelsY / 8)];
+ }
+ pMapDisplay->EraseMask &= ~SCREEN_BIT(SCREEN_LARGE);
+ pMapDisplay->EraseMask |= MENUICON_BITS;
+ pMapDisplay->UpdateMask |= (MENUICON_BITS | SPECIAL_BIT(FRAME_SELECT) | SPECIAL_BIT(MENUTEXT));
+ }
+ if (Action == MENU_EXIT)
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ VarsUi.State = 0;
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* cUiPowerOffTime ****************************************************
+
+UBYTE cUiPowerOffTime(UBYTE Action) // MENU_INIT,MENU_LEFT,MENU_RIGHT,MENU_EXIT
+{
+ switch (Action)
+ {
+ case MENU_INIT : // Init time counter and cursor bitmap
+ {
+ VarsUi.Counter = VarsUi.NVData.PowerdownCode + 1;
+
+ VarsUi.pTmp = (UBYTE*)Cursor;
+ for (VarsUi.Tmp = 0;(VarsUi.Tmp < SIZE_OF_CURSOR) && (VarsUi.Tmp < (UBYTE)SIZEOF_DATA(Cursor));VarsUi.Tmp++)
+ {
+ VarsUi.CursorTmp[VarsUi.Tmp] = *VarsUi.pTmp;
+ VarsUi.pTmp++;
+ }
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_LEFT : // Dec
+ {
+ cUiListLeft(POWER_OFF_TIME_STEPS,&VarsUi.Counter);
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_RIGHT : // Inc
+ {
+ cUiListRight(POWER_OFF_TIME_STEPS,&VarsUi.Counter);
+ Action = MENU_DRAW;
+ }
+ break;
+
+ case MENU_ENTER : // Enter
+ {
+ VarsUi.NVData.PowerdownCode = VarsUi.Counter - 1;
+ cUiNVWrite();
+ IOMapUi.SleepTimeout = PowerOffTimeSteps[VarsUi.NVData.PowerdownCode];
+ Action = MENU_EXIT;
+ }
+ break;
+
+ }
+
+ if (Action == MENU_DRAW)
+ {
+ if (VarsUi.Counter > 1)
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,"%u",(UWORD)PowerOffTimeSteps[VarsUi.Counter - 1]);
+ }
+ else
+ {
+ sprintf((char*)VarsUi.DisplayBuffer,(char*)cUiGetString(TXT_POWEROFFTIME_NEVER));
+ }
+ pMapDisplay->pTextLines[TEXTLINE_3] = VarsUi.DisplayBuffer;
+
+ pMapDisplay->pBitmaps[BITMAP_1] = (BMPMAP*)VarsUi.CursorTmp;
+ VarsUi.CursorTmp[4] = 46;
+ VarsUi.CursorTmp[5] = 24;
+ pMapDisplay->EraseMask |= (TEXTLINE_BIT(TEXTLINE_3) | TEXTLINE_BIT(TEXTLINE_4));
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= (TEXTLINE_BIT(TEXTLINE_3) | BITMAP_BIT(BITMAP_1));
+ }
+ if (Action == MENU_EXIT)
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ }
+
+ return (0);
+}
+
+
+
+//******* cUiBTConnectRequest ************************************************
+
+UBYTE cUiBTConnectRequest(UBYTE Action)
+{
+ switch (Action)
+ {
+ case MENU_INIT :
+ {
+ switch (VarsUi.CRState)
+ {
+ case 0 :
+ {
+ sprintf((char*)pMapSound->SoundFilename,"%s.%s",(char*)UI_ATTENTION_SOUND,(char*)TXT_FILE_EXT[FILETYPE_SOUND]);
+ pMapSound->Volume = IOMapUi.Volume;
+ pMapSound->Mode = SOUND_ONCE;
+ pMapSound->Flags |= SOUND_UPDATE;
+ VarsUi.CRState++;
+ }
+ break;
+
+ case 1 :
+ {
+ if (DISPLAY_IDLE)
+ {
+ pMapDisplay->Flags |= DISPLAY_POPUP;
+ VarsUi.CRState++;
+ }
+ }
+ break;
+
+ case 2 :
+ {
+ strcpy((char*)VarsUi.UserString,(char*)DEFAULT_PIN_CODE);
+ IOMapUi.Flags |= UI_REDRAW_STATUS;
+ VarsUi.CRState++;
+ }
+ break;
+
+ case 3 : // Get pincode and send
+ {
+ if (!cUiGetUserString(0))
+ {
+ if (VarsUi.UserString[0] == 0)
+ {
+ sprintf((char*)VarsUi.UserString,"%08lX",VarsUi.CRPasskey);
+ }
+ pMapComm->pFunc2(VarsUi.UserString);
+ VarsUi.CRState++;
+ }
+ }
+ break;
+
+ case 4 :
+ {
+ if (DISPLAY_IDLE)
+ {
+ pMapDisplay->Flags &= ~DISPLAY_POPUP;
+ VarsUi.CRState = 0;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.CRState);
+}
+
+
+
+//******* cUiFilesDelete *****************************************************
+
+UBYTE cUiFilesDelete(UBYTE Action)
+{
+ switch (Action)
+ {
+ case MENU_INIT :
+ {
+ pMapDisplay->pTextLines[TEXTLINE_3] = cUiGetString(TXT_FILESDELETE_DELETING_ALL);
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_3);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_3);
+ sprintf((char*)VarsUi.DisplayBuffer,(char*)cUiGetString(TXT_FILESDELETE_S_FILES),(char*)cUiGetString(TXT_FILETYPE[VarsUi.SelectedType]));
+ pMapDisplay->pTextLines[TEXTLINE_4] = VarsUi.DisplayBuffer;
+ pMapDisplay->TextLinesCenterFlags |= TEXTLINE_BIT(TEXTLINE_4);
+ pMapDisplay->UpdateMask |= TEXTLINE_BIT(TEXTLINE_4);
+ IOMapUi.State = TEST_BUTTONS;
+ }
+ break;
+
+ case MENU_DELETE :
+ {
+ switch (VarsUi.State)
+ {
+ case 0 :
+ {
+ if (VarsUi.SelectedType < FILETYPES)
+ {
+ sprintf((char*)VarsUi.FilenameBuffer,"*.%s",TXT_FILE_EXT[VarsUi.SelectedType]);
+ }
+ else
+ {
+ sprintf((char*)VarsUi.FilenameBuffer,"*.*");
+ }
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 :
+ {
+ if (SOUND_IDLE == pMapSound->State)
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 : // Delete files
+ {
+ VarsUi.TmpHandle = pMapLoader->pFunc(FINDFIRST,VarsUi.FilenameBuffer,VarsUi.SelectedFilename,&VarsUi.TmpLength);
+ if (!(VarsUi.TmpHandle & 0x8000))
+ {
+ pMapLoader->pFunc(CLOSE,(UBYTE*)&VarsUi.TmpHandle,NULL,NULL);
+ pMapLoader->pFunc(DELETE,VarsUi.SelectedFilename,NULL,NULL);
+ }
+ else
+ {
+ pMapDisplay->EraseMask |= MENUICON_BITS;
+ pMapDisplay->EraseMask |= SPECIAL_BIT(MENUTEXT);
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default : // Display Files deleted text
+ {
+ if (!cUiFeedback((BMPMAP*)Info,TXT_FB_FD_FILES_INFO,TXT_FB_FD_DELETED_INFO,DISPLAY_SHOW_TIME))
+ {
+ IOMapUi.State = EXIT_PRESSED;
+ VarsUi.State = 0;
+ }
+ }
+ break;
+
+ }
+ }
+ break;
+
+ default :
+ {
+ if (Action < FILETYPES)
+ {
+ VarsUi.SelectedType = Action;
+ }
+ else
+ {
+ VarsUi.SelectedType = FILETYPE_ALL;
+ }
+ }
+ break;
+
+ }
+
+ return (VarsUi.State);
+}
+
+
+//******* cUiOff *************************************************************
+
+UBYTE cUiOff(UBYTE Action) // Tell AVR to turn off ARM
+{
+ if (Action == MENU_INIT)
+ {
+ switch (VarsUi.State)
+ {
+ case 0 : // Stop VM if running
+ {
+ if (pMapCmd->ProgStatus == PROG_RUNNING)
+ {
+ pMapCmd->DeactivateFlag = TRUE;
+ }
+ VarsUi.State++;
+ }
+ break;
+
+ case 1 : // When VM is stopped -> Display off and close all connections
+ {
+ if (pMapCmd->ProgStatus != PROG_RUNNING)
+ {
+ pMapDisplay->Flags &= ~DISPLAY_ON;
+ VarsUi.BTCommand = (UBYTE)DISCONNECTALL;
+ VarsUi.BTPar1 = (UBYTE)0;
+ VarsUi.BTPar2 = (UBYTE)0;
+ pMapComm->pFunc(VarsUi.BTCommand,VarsUi.BTPar1,VarsUi.BTPar2,0,NULL,&(VarsUi.BTResult));
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 2 : // Send off command to AVR
+ {
+ if (VarsUi.BTResult != INPROGRESS)
+ {
+ pMapIoCtrl->PowerOn = POWERDOWN;
+ VarsUi.Timer = 0;
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 3 : // Wait for power off
+ {
+ if (++VarsUi.Timer >= ARM_WAIT_FOR_POWER_OFF)
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ case 4 : // Vitual off state (if still power) wait for on button
+ {
+ pMapIoCtrl->PowerOn = 0;
+ if (BUTTON_ENTER == cUiReadButtons())
+ {
+ VarsUi.State++;
+ }
+ }
+ break;
+
+ default : // Turn on again
+ {
+ IOMapUi.State = INIT_DISPLAY;
+ VarsUi.State = 0;
+ }
+ break;
+
+ }
+ }
+
+ return (VarsUi.State);
+}
+
+
+
+//******* FUNCTIONS **********************************************************
+
+enum FUNC_NO // Must reffer to entry in Functions
+{ // used in Menus to repressent function
+ FUNC_NO_NOT_USED = 0x00,
+ FUNC_NO_TEST_PROGRAM = 0x01,
+ FUNC_NO_OFF = 0x02,
+ FUNC_NO_BT_ON = 0x03,
+ FUNC_NO_POWER_OFF_TIME = 0x04,
+ FUNC_NO_FILES_DELETE = 0x05,
+ FUNC_NO_FILE_LIST = 0x06,
+ FUNC_NO_VOLUME = 0x07,
+ FUNC_NO_FILE_RUN = 0x08,
+ FUNC_NO_FILE_DELETE = 0x09,
+ FUNC_NO_FREE1 = 0x0A,
+ FUNC_NO_ON_BRICK_PROGRAMMING = 0x0B,
+ FUNC_NO_FREE2 = 0x0C,
+ FUNC_NO_BT_CONNECT_REQUEST = 0x0D,
+ FUNC_NO_VIEW = 0x0E,
+ FUNC_NO_GET_USER_STRING = 0x0F,
+ FUNC_NO_BT_CONNECT = 0x10,
+ FUNC_NO_BT_VISIABILITY = 0x11,
+ FUNC_NO_BT_SEARCH = 0x12,
+ FUNC_NO_BT_DEVICE_LIST = 0x13,
+ FUNC_NO_BT_CONNECT_LIST = 0x14,
+ FUNC_NO_MAX
+};
+
+FUNCTION Functions[] = // Use same index as FUNC_NO
+{
+ 0,
+ TestPrg,
+ cUiOff,
+ cUiBtOn,
+ cUiPowerOffTime,
+ cUiFilesDelete,
+ cUiFileList,
+ cUiVolume,
+ cUiFileRun,
+ cUiFileDelete,
+ cUiDataLogging,
+ cUiOnBrickProgramming,
+ 0,
+ cUiBTConnectRequest,
+ cUiView,
+ cUiGetUserString,
+ cUiBtConnect,
+ cUiBtVisiability,
+ cUiBtSearch,
+ cUiBtDeviceList,
+ cUiBtConnectList
+};
+
+
diff --git a/AT91SAM7S256/Source/Icons.txt b/AT91SAM7S256/Source/Icons.txt
new file mode 100644
index 0000000..459b078
--- /dev/null
+++ b/AT91SAM7S256/Source/Icons.txt
@@ -0,0 +1,293 @@
+DEFINE_DATA(ICON, Icons) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x1A,0x70, // Graphics DataSize
+ 0x01, // Graphics Count X
+ 0x5E, // Graphics Count Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xA0,0x50,0xB0,0x50,0xB0,0x50,0xA0,0xC0,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0x88,0x47,0x2F,0x1F,0x1E,0x1F,0x1E,0x1D,0x0E,0x07,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x06,0x09,0x08,0x04,0x02,0x01,0x08,0x14,0x1F,0x00,0x1F,0x15,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xA0,0x50,0xB0,0x50,0xB0,0x50,0xA0,0xC0,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0x88,0x47,0x2F,0x1F,0x1E,0x1F,0x1E,0x1D,0x0E,0x07,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x06,0x09,0x08,0x04,0x02,0x01,0x08,0x14,0x1F,0x00,0x1F,0x15,0x0A,0x00,0x1E,0x09,0x1E,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x98,0x40,0x40,0x20,0x20,0x20,0x40,0x40,0x98,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x02,0x42,0x20,0x07,0x18,0x60,0x80,0x00,0x00,0x00,0x80,0x60,0x18,0x07,0x00,0x10,0x22,0x02,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0A,0x15,0x35,0x25,0x35,0x15,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0xC0,0x60,0xA0,0x60,0xC0,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x1A,0x75,0xAA,0x55,0xAA,0x55,0xAA,0x75,0x1A,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0A,0x15,0x35,0x25,0x35,0x15,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xFE,0x02,0xC3,0xC2,0xC2,0x02,0xC3,0x42,0xC2,0x03,0x02,0x02,0x02,0x03,0x02,0xFE,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x1F,0x1C,0x1D,0x1D,0x1D,0x1C,0x1D,0x1D,0x1D,0x1C,0x1C,0x1C,0x1C,0x1C,0x1C,0x1F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,
+ 0x00,0x00,0x08,0x04,0x04,0x02,0x09,0x09,0x4C,0x4E,0xCB,0xC8,0xC8,0xC8,0xC8,0x70,0x00,0x00,0xF8,0x04,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x18,0x18,0x30,0x30,0x30,0x34,0x35,0x1D,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x80,0xC0,0x80,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x10,0xBA,0xC6,0x01,0x11,0x38,0x6C,0x38,0x11,0x01,0xC6,0xBA,0x10,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x02,0x06,0x02,0x01,0x03,0x00,0x00,0x1E,0x12,0x0C,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x80,0xC0,0x80,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x10,0xBA,0xC6,0x01,0x11,0x38,0x6C,0x38,0x11,0x01,0xC6,0xBA,0x10,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x02,0x06,0x02,0x01,0x03,0x00,0x00,0x1E,0x0A,0x14,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xE0,0xC0,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xF8,0x06,0x01,0x01,0x00,0x03,0x01,0x00,0x00,0x01,0x01,0x06,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x04,0x08,0x08,0x08,0x08,0x08,0x04,0x04,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xE0,0x3C,0xC3,0x00,0x00,0x00,0xFF,0x10,0x10,0x82,0x6C,0x10,0x01,0xC6,0x38,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x3F,0x00,0x00,0x01,0x02,0x04,0x07,0x00,0x00,0x1C,0x00,0x1C,0x05,0x08,0x1C,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xE0,0x3C,0xC3,0x00,0x00,0x00,0xFF,0x10,0x10,0x82,0x6C,0x10,0x01,0xC6,0x38,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x3F,0x00,0x00,0x01,0x02,0x04,0x07,0x00,0x1C,0x14,0x00,0x1C,0x05,0x08,0x04,0x1C,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x30,0x48,0x48,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0xC0,0x40,0x40,0x40,0x4F,0xD0,0x50,0x50,0x49,0x40,0xFC,0x04,0x07,0x05,0x05,0x05,0x07,0x04,0x04,0x07,0x00,0x00,
+ 0x00,0x00,0x07,0x06,0x06,0x06,0x06,0x07,0x04,0x04,0x04,0x04,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x30,0x48,0x48,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0xC0,0x40,0x40,0x40,0x5F,0xC4,0x44,0x40,0x40,0x40,0xFC,0x04,0x07,0x05,0x05,0x05,0x07,0x04,0x04,0x07,0x00,0x00,
+ 0x00,0x00,0x07,0x06,0x06,0x06,0x06,0x07,0x04,0x04,0x04,0x04,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0x08,0xF0,0x00,0x00,0x00,0x00,0x60,0x90,0x90,0x60,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x7F,0x04,0x7F,0x80,0x00,0x00,0x00,0xC0,0x20,0x20,0x40,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x08,0x07,0x00,0x00,0x03,0x04,0x04,0x02,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0x08,0xF0,0x00,0x00,0x00,0x00,0x60,0x90,0x90,0x60,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x7F,0x04,0x7F,0x80,0x00,0x00,0x00,0xE0,0x20,0x20,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x08,0x07,0x00,0x00,0x07,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x40,0xA0,0x60,0xA0,0x40,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xC0,0x30,0x18,0x0F,0x0A,0x0D,0x1A,0x35,0xEA,0xF5,0xFA,0xFF,0xF8,0xF8,0xF0,0xE0,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x01,0x06,0x04,0x08,0x08,0x08,0x04,0x06,0x01,0x03,0x07,0x07,0x07,0x07,0x03,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x3C,0x3C,0x3C,0x24,0x25,0x3D,0x25,0x24,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x3C,0x3D,0x3D,0x25,0x24,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x24,0x25,0x3D,0x3D,0x3C,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x24,0x25,0x3D,0x25,0x24,0x3C,0x3C,0x3C,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x78,0x78,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x24,0x25,0x3D,0x25,0x24,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x78,0x78,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x24,0x25,0x3D,0x25,0x24,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x78,0x78,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x3C,0x24,0x25,0x3D,0x25,0x24,0x3C,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x1F,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x1F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x10,0x10,0x10,0x10,0x90,0x90,0x50,0x50,0xD0,0x10,0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x1F,0x20,0x20,0x20,0x2C,0x2F,0x20,0x20,0x26,0x27,0x20,0x20,0x20,0x1F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x10,0x10,0x10,0xD0,0x50,0x50,0x50,0x50,0xD0,0x10,0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x1F,0x20,0x20,0x20,0x2F,0x28,0x23,0x23,0x28,0x2F,0x20,0x20,0x20,0x1F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x10,0xD0,0x50,0xD0,0x90,0x50,0x50,0x90,0xD0,0x50,0xD0,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x3F,0x40,0x41,0x41,0x41,0x40,0x41,0x41,0x40,0x41,0x41,0x41,0x40,0x3F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x10,0x10,0x10,0x90,0x50,0x70,0x50,0x70,0x50,0x90,0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x1F,0x20,0x20,0x20,0x2F,0x30,0x25,0x28,0x25,0x30,0x2F,0x20,0x20,0x1F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF0,0x10,0xF0,0x90,0xD0,0x90,0x10,0x10,0x10,0x10,0x10,0x50,0x10,0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x3F,0x40,0x5F,0x50,0x54,0x50,0x52,0x50,0x51,0x50,0x78,0x50,0x40,0x3F,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x08,0x10,0x10,0x10,0x10,0x1F,0x11,0x1D,0x1D,0x11,0x11,0x11,0x1F,0x10,0x10,0x10,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x04,0x06,0xFD,0x00,0x00,0x00,0x00,0xFD,0x06,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x10,0x10,0x10,0x10,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x40,0x80,0x00,0x00,0x00,0x58,0x68,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x04,0x06,0xFD,0x00,0x00,0x00,0x00,0xFD,0x06,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x10,0x10,0x10,0x10,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xE0,0x20,0x20,0x20,0x20,0xE0,0x00,0x68,0x58,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x60,0x90,0x08,0x04,0x0E,0x08,0x08,0x0F,0x00,0x00,0x00,0x80,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x07,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0xE0,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x06,0x09,0x10,0x20,0x70,0x10,0x10,0xF0,0x00,0x00,0x00,0x01,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0xE0,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x68,0x58,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x06,0x09,0x10,0x20,0x70,0x10,0x10,0xF0,0x00,0x00,0x00,0x01,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xE0,0x20,0x20,0x20,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0x80,0x00,0x00,0x00,0x0F,0x08,0x08,0x0E,0x04,0x08,0x90,0x60,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x07,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0xE0,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x01,0x00,0x00,0x00,0xF0,0x10,0x10,0x70,0x20,0x10,0x09,0x06,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x68,0x58,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0xE0,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x01,0x00,0x00,0x00,0xF0,0x10,0x10,0x70,0x20,0x10,0x09,0x06,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xE0,0x20,0x20,0x20,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x60,0x90,0x08,0x04,0x0E,0x08,0x08,0x0F,0x00,0x00,0x00,0x80,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x07,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x40,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x03,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x40,0x40,0x00,0x00,0x00,0xC0,0x60,0x20,0x20,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x7F,0x00,0x00,0x80,0xC0,0xC0,0xC0,0xBF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x03,0x03,0x01,0x00,0x00,0x00,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x40,0x40,0x40,0x40,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x05,0x08,0x10,0x10,0x08,0x05,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x40,0x40,0x40,0x40,0xC0,0x00,0x00,0x58,0x68,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x05,0x08,0x10,0x10,0x08,0x05,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x68,0x58,0x00,0xE0,0x20,0x20,0x20,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0x80,0x00,0x00,0x00,0x0F,0x08,0x08,0x0E,0x04,0x08,0x90,0x60,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x07,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x40,0xA0,0x10,0x20,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0xA0,0x40,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x82,0x44,0x28,0x11,0x82,0x44,0x82,0x11,0x28,0x44,0x82,0x01,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x04,0x0A,0x11,0x08,0x04,0x02,0x01,0x00,0x00,0x00,0x01,0x02,0x04,0x08,0x11,0x0A,0x04,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x60,0x10,0x90,0x60,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xC0,0x20,0x20,0x40,0x40,0x80,0x80,0x80,0x60,0x18,0x86,0x61,0x18,0x06,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x03,0x04,0x04,0x08,0x08,0x10,0x10,0x20,0x20,0x18,0x06,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xA8,0xA8,0x54,0x54,0x2A,0x2A,0x00,0x3F,0x21,0x32,0x2C,0x24,0x24,0x24,0x2C,0x32,0x21,0x3F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x40,0x40,0x60,0x60,0x60,0x40,0x40,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0xFF,0x01,0xFF,0x01,0xFF,0x01,0xFF,0x01,0xFF,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x08,0x0F,0x08,0x0F,0x08,0x0F,0x08,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x01,0x01,0x02,0x02,0x04,0x04,0x88,0x88,0x50,0x50,0x20,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x0F,0x08,0x04,0x04,0x02,0x02,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x20,0x50,0x50,0x88,0x88,0x04,0x04,0x02,0x02,0x01,0x01,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x02,0x02,0x04,0x04,0x08,0x0F,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xFF,0x01,0x01,0x01,0x01,0x7F,0x40,0x48,0x4C,0x7A,0x01,0x00,0x00,0x01,0xFA,0x0C,0x08,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x03,0x04,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x04,0x03,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0xC0,0xC0,0xC0,0xC0,0xC0,0x80,0x80,0x60,0xE0,0xC0,0x80,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xF8,0xFE,0x07,0x21,0x21,0x20,0x20,0xE0,0x00,0x00,0x01,0x01,0x07,0xFE,0xF9,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x03,0x07,0x0C,0x0C,0x18,0x18,0x1B,0x18,0x18,0x0C,0x0C,0x07,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x80,0x00,0x20,0xC0,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x78,0x48,0x48,0x84,0x02,0x01,0xFF,0x00,0x00,0x86,0x78,0x01,0x86,0x78,0x01,0x86,0x78,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x07,0x00,0x00,0x01,0x00,0x06,0x01,0x10,0x0E,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0xE0,0xE0,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x78,0xFE,0x87,0x01,0x01,0x00,0x07,0x07,0x00,0x01,0x01,0x87,0xFE,0x78,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x06,0x06,0x0C,0x0C,0x0C,0x0C,0x06,0x06,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0xF8,0x08,0xF8,0x48,0xC8,0x48,0x48,0x48,0x48,0x48,0x48,0x48,0x78,0x48,0xF8,0x40,0xC0,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x02,0xFE,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x03,0x02,0xFF,0x02,0xFE,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x01,0x07,0x08,0x3F,0x40,0x80,0x81,0xF9,0x89,0xE9,0x89,0x89,0x89,0xF9,0x81,0x80,0x80,0x7F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0x20,0x20,0x20,0xE0,0x80,0x80,0xE0,0x20,0x20,0x20,0xE0,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x03,0x02,0x02,0x02,0x03,0x00,0x00,0x03,0x02,0x82,0xCA,0xFB,0x78,0x78,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x7C,0x44,0x44,0x44,0x7C,0x00,0x03,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x08,0xFC,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x60,0x60,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x60,0x60,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x1F,0x10,0x10,0x13,0x13,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x38,0x10,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0x30,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x30,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x80,0xC0,0xA0,0x90,0x08,0x08,0x90,0xA0,0xC0,0x80,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x01,0x03,0x02,0x02,0x02,0x02,0x02,0x1F,0x10,0x10,0x1F,0x02,0x02,0x02,0x02,0x02,0x03,0x01,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0xFE,0xFF,0xFF,0xE3,0xE1,0xE0,0xF0,0x78,0x38,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x1C,0x1E,0x1F,0x0F,0x07,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0xE0,0xA0,0xA0,0xA0,0xA0,0xA0,0xA0,0xE0,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xFE,0x01,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x01,0xFE,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x0F,0x10,0x20,0x20,0x20,0x22,0x24,0x24,0x24,0x24,0x22,0x20,0x20,0x20,0x10,0x0F,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x40,0x40,0x40,0x40,0x40,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xF8,0x06,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x06,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x08,0x08,0x10,0x10,0x10,0x10,0x10,0x08,0x08,0x04,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xE0,0x3C,0xC3,0x00,0x00,0x00,0xFF,0x10,0x10,0x82,0x6C,0x10,0x01,0xC6,0x38,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x7F,0x00,0x00,0x01,0x02,0x04,0x07,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xA0,0x50,0xB0,0x50,0xB0,0x50,0xA0,0xC0,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0x88,0x47,0x2F,0x1F,0x1F,0x1F,0x1E,0x1F,0x0E,0x07,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x06,0x09,0x08,0x04,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,
+ 0x00,0x00,0x08,0x04,0x04,0x02,0x09,0x09,0x4C,0x4E,0xCB,0xC8,0xC8,0xC8,0xC8,0x70,0x00,0x00,0xF8,0x04,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x18,0x18,0x30,0x30,0x30,0x34,0x35,0x1D,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x38,0xC8,0x08,0x08,0x08,0x08,0xA8,0x68,0x08,0x08,0x08,0x08,0xC8,0x38,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x81,0x42,0x24,0x98,0x85,0x85,0x98,0x24,0x42,0x81,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x1C,0x13,0x14,0x17,0x17,0x17,0x17,0x17,0x17,0x17,0x17,0x14,0x13,0x1C,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x38,0xC8,0x08,0x08,0x08,0x08,0x68,0xA8,0x08,0x08,0x08,0x08,0xC8,0x38,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x81,0x42,0x24,0x18,0x05,0x05,0x18,0x24,0x42,0x81,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x1C,0x13,0x14,0x16,0x17,0x17,0x17,0x17,0x17,0x17,0x16,0x14,0x13,0x1C,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x38,0xC8,0x08,0x08,0x48,0xE8,0x08,0xC8,0x28,0xC8,0x08,0x08,0xC8,0x38,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x81,0x42,0x24,0x19,0x04,0x04,0x19,0x24,0x42,0x81,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x1C,0x13,0x18,0x1C,0x1C,0x1C,0x1C,0x1C,0x1C,0x1C,0x1C,0x18,0x13,0x1C,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x40,0x40,0x40,0x40,0x40,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xF8,0x06,0xF9,0x56,0xAA,0x55,0xAB,0x55,0xAB,0x55,0xAA,0x56,0xF9,0x06,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x0B,0x0A,0x15,0x16,0x15,0x16,0x15,0x0A,0x0B,0x04,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x10,0x20,0x40,0x80,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x5C,0x74,0x00,0x04,0x7C,0x04,0x00,0x38,0x44,0x38,0x00,0x7C,0x14,0x08,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x01,0x02,0x04,0x08,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x08,0x04,0x02,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x40,0x40,0x60,0xFC,0xF8,0xF0,0x60,0x00,0x00,0x40,0x40,0x80,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xFC,0x03,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x80,0x00,0x00,0x00,0x00,0x00,0x81,0x7E,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x04,0x00,0x00,0x0C,0x1E,0x3F,0x7F,0x04,0x04,0x04,0x02,0x02,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0x78,0x48,0x48,0xF8,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x9E,0x91,0x51,0x51,0x51,0x91,0x9E,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x24,0x24,0x24,0x3C,0x25,0x25,0x25,0x3C,0x24,0x24,0x24,0x3F,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x60,0x30,0x10,0x10,0x90,0x10,0x10,0x30,0x60,0xC0,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x82,0x44,0x28,0xFF,0x11,0xAA,0x44,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x03,0x06,0x0C,0x18,0x10,0x10,0x13,0x11,0x10,0x18,0x0C,0x06,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x40,0xC0,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0x00,0x55,0x00,0x55,0x55,0x55,0x00,0x55,0xFF,0x00,0x01,0x00,0x01,0x01,0x01,0x00,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x0F,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x12,0x15,0x17,0x12,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0F,0x00,0x00,0x00,
+ 0x00,0x00,0x80,0x80,0x80,0x80,0x40,0x40,0x40,0x80,0x40,0x40,0x40,0x40,0x40,0x40,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x04,0x04,0x02,0x42,0xC6,0x88,0x10,0x60,0xC0,0x01,0x81,0x81,0x81,0x00,0x00,
+ 0x00,0x00,0x01,0x01,0x01,0x02,0x04,0x08,0x0A,0x0C,0x09,0x0B,0x0E,0x0C,0x05,0x07,0x02,0x03,0x01,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x60,0xE2,0xB2,0xF6,0xF6,0x76,0x2C,0x6C,0xDC,0x9C,0x38,0x30,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x04,0x05,0x05,0x05,0x04,0x02,0x02,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0xE0,0xE0,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x78,0x86,0x01,0x00,0x88,0x50,0xFD,0xA9,0x50,0x00,0x00,0x01,0x86,0x78,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x04,0x04,0x08,0x09,0x08,0x08,0x04,0x04,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xC0,0xE0,0x70,0x38,0x98,0x18,0xD8,0x98,0x18,0x38,0x70,0xE0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x1F,0x3F,0x70,0xE0,0xC8,0xC5,0xDF,0xCA,0xC5,0x60,0x70,0xFF,0xCF,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x07,0x0E,0x1C,0x18,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x82,0x44,0x28,0xFF,0x11,0xAA,0x44,0x00,0x00,0x06,0x01,0x00,0x40,0x20,0x11,0x0E,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x82,0x44,0x28,0xFF,0x11,0xAA,0x44,0x00,0x00,0x00,0x00,0x01,0x82,0x44,0x28,0x10,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x83,0xC6,0x6C,0xFF,0x39,0xBA,0x6C,0xC6,0x83,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x00,0x00,0x03,0x01,0x00,0x00,0x00,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x60,0xE2,0xB2,0xF6,0xF6,0x76,0x2C,0x6C,0xDC,0x9C,0x38,0x30,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x04,0x05,0x05,0x05,0x04,0x02,0x02,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0xC0,0xE0,0x80,0x00,0x02,0x02,0x06,0x06,0x06,0x0C,0x0C,0x9C,0xDC,0xB8,0x30,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x03,0x02,0x02,0x02,0x02,0x02,0x03,0x03,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x00,0xE0,0xE0,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x78,0x86,0x01,0x00,0x88,0x50,0xFD,0xA9,0x50,0x00,0x00,0x01,0x86,0x78,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x04,0x04,0x08,0x09,0x08,0x08,0x04,0x04,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x80,0x40,0x20,0x10,0x20,0x40,0x80,0xE0,0xE0,0x00,0x80,0x40,0x20,0x10,0x20,0x40,0x80,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x79,0x86,0x44,0x28,0x10,0x00,0x01,0x83,0x01,0x00,0x10,0x28,0x44,0xFA,0x01,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x02,0x05,0x08,0x10,0x08,0x04,0x0A,0x09,0x08,0x09,0x06,0x04,0x08,0x10,0x08,0x05,0x02,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Info.txt b/AT91SAM7S256/Source/Info.txt
new file mode 100644
index 0000000..7e2b639
--- /dev/null
+++ b/AT91SAM7S256/Source/Info.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, Info) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x48, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x08, // Graphics Start Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x40,0x80,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x40,0xA0,0x90,0x20,0x20,0x40,0x40,0x30,0x8C,0x73,0x0C,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x02,0x02,0x02,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/LowBattery.txt b/AT91SAM7S256/Source/LowBattery.txt
new file mode 100644
index 0000000..51b8ddb
--- /dev/null
+++ b/AT91SAM7S256/Source/LowBattery.txt
@@ -0,0 +1,18 @@
+DEFINE_DATA(BMPMAP, LowBattery) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x02,0xA0, // Graphics DataSize
+ 0x02, // Graphics Start X
+ 0x08, // Graphics Start Y
+ 0x60, // Graphics Width
+ 0x38, // Graphics Height
+BEGIN_DATA
+ 0x02,0xF2,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x92,0x92,0x92,0x92,0x92,0x92,0x92,0x92,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0x12,0xF2,0x82,0x02,
+ 0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x20,0x18,0x04,0x02,0x02,0x01,0x01,0x01,0x00,0xF0,0xF8,0xFC,0xFC,0xF8,0xF0,0x00,0x01,0x01,0x01,0x02,0x02,0x04,0x18,0x20,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,
+ 0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x7F,0xFF,0xFF,0x7F,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,
+ 0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x0E,0x10,0x60,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x60,0xF0,0xF0,0xF0,0xF0,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x60,0x10,0x0E,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,
+ 0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0x00,0x00,0x00,0x00,0x00,0xE0,0x10,0x10,0x10,0xE0,0x00,0xF0,0x00,0xC0,0x00,0xF0,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0xF2,0x92,0x92,0x94,0x64,0x04,0xE4,0x14,0x14,0x14,0xE4,0x02,0x12,0x12,0xF1,0x11,0x10,0x00,0x10,0x10,0xF0,0x10,0x10,0x00,0xF0,0x90,0x90,0x90,0x10,0x00,0xF0,0x90,0x90,0x90,0x60,0x00,0x70,0x80,0x00,0x80,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,
+ 0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x00,0x03,0x04,0x04,0x04,0x03,0x00,0x03,0x04,0x03,0x04,0x03,0x00,0x00,0x00,0x00,0xC0,0x40,0xF0,0x17,0x14,0x14,0x14,0x13,0x10,0x17,0x11,0x11,0x11,0x17,0x10,0x10,0xD0,0xD7,0x10,0xF0,0x00,0x00,0x00,0x07,0x00,0x00,0x00,0x07,0x04,0x04,0x04,0x04,0x00,0x07,0x00,0x00,0x01,0x06,0x00,0x00,0x00,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,
+ 0x00,0x3F,0x20,0x20,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x63,0x62,0x6F,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x68,0x6B,0x6B,0x68,0x6F,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x7F,0x7F,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Mainmenu.rms b/AT91SAM7S256/Source/Mainmenu.rms
new file mode 100644
index 0000000..2e014fb
--- /dev/null
+++ b/AT91SAM7S256/Source/Mainmenu.rms
@@ -0,0 +1,72 @@
+const UBYTE MAINMENU[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x01,0x05, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x09, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Turn_off?
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x10,0x20,0x04,0x01, // 0x10200401
+ 0x02,0x00,0x00,0x01, // 2 ,0 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6F,0x66,0x66,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Turn_off?
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x10,0x20,0x00,0x01, // 0x10200001
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6F,0x66,0x66,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x30, // 30
+
+ // My_Files
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x00,0x00,0x01,0x01, // 0 ,0 ,1 ,1
+ 0x4D,0x79,0x20,0x46,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x3B, // 3B
+
+ // NXT_Program
+ 0x00,0x00,0x00,0x21, // 0x00000021
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x00,0x00,0x02,0x01, // 0 ,0 ,2 ,1
+ 0x4E,0x58,0x54,0x20,0x50,0x72,0x6F,0x67,0x72,0x61,0x6D,0x00,0x00,0x00,0x00,0x00,
+ 0x3C, // 3C
+
+ // NXT_Datalog
+ 0x00,0x00,0x00,0x31, // 0x00000031
+ 0x01,0x84,0x00,0x00, // 0x01840000
+ 0x0A,0x00,0x03,0x01, // 10 ,0 ,3 ,1
+ 0x4E,0x58,0x54,0x20,0x44,0x61,0x74,0x61,0x6C,0x6F,0x67,0x00,0x00,0x00,0x00,0x00,
+ 0x3D, // 3D
+
+ // View
+ 0x00,0x00,0x00,0x41, // 0x00000041
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x0E,0x00,0x04,0x01, // 14 ,0 ,4 ,1
+ 0x56,0x69,0x65,0x77,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x3E, // 3E
+
+ // Bluetooth
+ 0x00,0x00,0x00,0x51, // 0x00000051
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x00,0x00,0x07,0x02, // 0 ,0 ,7 ,2
+ 0x42,0x6C,0x75,0x65,0x74,0x6F,0x6F,0x74,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x51, // 51
+
+ // Settings
+ 0x00,0x00,0x00,0x61, // 0x00000061
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x00,0x00,0x05,0x01, // 0 ,0 ,5 ,1
+ 0x53,0x65,0x74,0x74,0x69,0x6E,0x67,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x3F, // 3F
+
+ // Try_Me
+ 0x00,0x00,0x00,0x71, // 0x00000071
+ 0x01,0x04,0x00,0x00, // 0x01040000
+ 0x00,0x00,0x06,0x01, // 0 ,0 ,6 ,1
+ 0x54,0x72,0x79,0x20,0x4D,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x40, // 40
+};
diff --git a/AT91SAM7S256/Source/Ok.txt b/AT91SAM7S256/Source/Ok.txt
new file mode 100644
index 0000000..32bad41
--- /dev/null
+++ b/AT91SAM7S256/Source/Ok.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, Ok) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x20, // Graphics DataSize
+ 0x2A, // Graphics Start X
+ 0x30, // Graphics Start Y
+ 0x10, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x38,0xC4,0x34,0x08,0x00,0xFF,
+ 0xFF,0x04,0x0A,0x19,0x12,0x22,0x24,0x24,0x23,0x18,0x07,0x00,0x00,0x00,0x00,0xFF
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Port.txt b/AT91SAM7S256/Source/Port.txt
new file mode 100644
index 0000000..292fccc
--- /dev/null
+++ b/AT91SAM7S256/Source/Port.txt
@@ -0,0 +1,12 @@
+DEFINE_DATA(ICON, Port) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x00,0x18, // Graphics DataSize
+ 0x08, // Graphics Count X
+ 0x01, // Graphics Count Y
+ 0x03, // Graphics Width
+ 0x08, // Graphics Height
+BEGIN_DATA
+ 0x70,0x88,0x70,0x90,0xF8,0x80,0xC8,0xA8,0x90,0x88,0xA8,0x50,0x38,0x20,0xF8,0xF0,0x28,0xF0,0xF8,0xA8,0x50,0x70,0x88,0x50
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_1.txt b/AT91SAM7S256/Source/RCXintro_1.txt
new file mode 100644
index 0000000..456a63d
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_1.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, RCXintro_1) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x02,0x00, // Graphics DataSize
+ 0x10, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x40, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0xFF,0x01,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0x01,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x8F,0xC7,0xE7,0x67,0x33,0x33,0x33,0x67,0xE7,0x8F,0xCF,0xE7,0x67,0x63,0x33,0x33,0x33,0x33,0x33,0x27,0x67,0xCF,0xCF,0xE7,0x67,0x63,0x33,0x33,0x33,0x33,0x33,0x63,0x67,0xC7,0x8F,0x8F,0xCF,0x67,0x67,0x23,0x33,0x33,0x33,0x33,0x63,0x67,0xC7,0x8F,0x1F,0x33,0xF3,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0x7F,0x0F,0x83,0xE0,0x78,0x1E,0x07,0x01,0x00,0x00,0x80,0xE0,0xF8,0x7E,0x1F,0x07,0x01,0x00,0x00,0x00,0x18,0x18,0x18,0xF8,0xF8,0x1C,0x06,0x03,0x00,0x00,0xC0,0x70,0x3C,0x3C,0x30,0x20,0x20,0xE0,0xF0,0x3D,0x0F,0x03,0x00,0x00,0xC0,0xF0,0xFC,0x3C,0x00,0x00,0x00,0x00,0xC0,0xFF,0x1E,0x00,0xC0,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0x01,0x00,0xFC,0xFF,0x03,0x00,0x00,0x00,0x00,0x00,0x0E,0x0F,0xFF,0xC7,0x00,0x00,0x00,0x00,0x00,0x0E,0x0E,0x0E,0x0E,0xFE,0xC7,0x80,0x00,0x00,0x00,0x00,0x1F,0x1F,0x0E,0x00,0x00,0x80,0xC0,0xF0,0xFF,0x83,0x00,0x00,0x00,0x1C,0x1F,0x0F,0x03,0x00,0x80,0xC0,0xE0,0x78,0x1E,0x87,0xC1,0xF0,0xFE,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFE,0xFC,0xF8,0xF3,0xF3,0xE6,0xE6,0xE6,0xE6,0xE6,0xF2,0xF3,0xF9,0xF9,0xF3,0xF6,0xE6,0xE6,0xE6,0xE6,0xE6,0xF3,0xF3,0xF9,0xF9,0xF3,0xF3,0xE6,0xE6,0xE6,0xE6,0xE6,0xE6,0xF3,0xF3,0xF9,0xFC,0xFC,0xF9,0xF3,0xF3,0xE6,0xE6,0xE6,0xE6,0xE6,0xE6,0xF3,0xF3,0xF9,0xF8,0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x80,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0x80,0xFF
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_10.txt b/AT91SAM7S256/Source/RCXintro_10.txt
new file mode 100644
index 0000000..5f98538
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_10.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_10) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x20, // Graphics DataSize
+ 0x38, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x0A, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xFE,0xFE,0x06,0x06,0x66,0x66,0x06,0x06,0xFE,0xFE,
+ 0x07,0x07,0x06,0x06,0x00,0x00,0x06,0x06,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_11.txt b/AT91SAM7S256/Source/RCXintro_11.txt
new file mode 100644
index 0000000..6afaabd
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_11.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_11) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x10, // Graphics DataSize
+ 0x3A, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x08, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,
+ 0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_12.txt b/AT91SAM7S256/Source/RCXintro_12.txt
new file mode 100644
index 0000000..b89f65c
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_12.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_12) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC0, // Graphics DataSize
+ 0x03, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x5E, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_13.txt b/AT91SAM7S256/Source/RCXintro_13.txt
new file mode 100644
index 0000000..ee512ea
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_13.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_13) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC0, // Graphics DataSize
+ 0x03, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x5E, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0x00,0x20,0x20,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0xE0,0xE0,0x00,0x00,0x00,0xC0,0xC0,
+ 0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_14.txt b/AT91SAM7S256/Source/RCXintro_14.txt
new file mode 100644
index 0000000..feb4cd7
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_14.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_14) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC0, // Graphics DataSize
+ 0x03, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x5E, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0x00,0xFE,0xFE,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0xD8,0xD8,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0x00,0x38,0x38,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0xD8,0xD8,
+ 0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x00,0x00,0x00,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_15.txt b/AT91SAM7S256/Source/RCXintro_15.txt
new file mode 100644
index 0000000..71f51e0
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_15.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_15) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC0, // Graphics DataSize
+ 0x03, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x5E, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xF8,0xF8,0x18,0x00,0xF8,0xF8,0x18,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x18,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x18,0x00,0x00,0xFE,0xFE,0x00,0x00,0xF8,0xF8,0x18,0x00,0xC0,0xD8,0xD8,0x00,0x00,0x18,0x18,0x00,0xF8,0xF8,0x18,0x00,0x00,0x00,0x00,0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x00,0x00,0x18,0x38,0x38,0x00,0x00,0xF8,0xF8,0x18,0x00,0xF8,0xF8,0x18,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x00,0xC0,0xD8,0xD8,
+ 0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x07,0x07,0x00,0x00,0x06,0x06,0x06,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x06,0x00,0x00,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_16.txt b/AT91SAM7S256/Source/RCXintro_16.txt
new file mode 100644
index 0000000..9cf470c
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_16.txt
@@ -0,0 +1,13 @@
+DEFINE_DATA(BMPMAP, RCXintro_16) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC0, // Graphics DataSize
+ 0x03, // Graphics Start X
+ 0x20, // Graphics Start Y
+ 0x5E, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xF8,0xF8,0x18,0x18,0xF8,0xF8,0x18,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x18,0x18,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x18,0x18,0x18,0xFE,0xFE,0x00,0x00,0xF8,0xF8,0xD8,0xD8,0xD8,0xD8,0xD8,0x00,0x00,0x18,0x18,0x18,0xF8,0xF8,0x18,0x18,0x18,0x00,0x00,0xF8,0xF8,0x18,0xD8,0xD8,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0x18,0x18,0x18,0x18,0x38,0x38,0x00,0x00,0xF8,0xF8,0x18,0x18,0xF8,0xF8,0x18,0x18,0xF8,0xF8,0x00,0x00,0xF8,0xF8,0xD8,0xD8,0xD8,0xD8,0xD8,
+ 0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x06,0x06,0x06,0x06,0x07,0x07,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x06,0x00,0x00,0x06,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x07,0x07,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_2.txt b/AT91SAM7S256/Source/RCXintro_2.txt
new file mode 100644
index 0000000..addecb2
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_2.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, RCXintro_2) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x02,0x00, // Graphics DataSize
+ 0x10, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x40, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0xFC,0x02,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0xFD,0x02,0xFC,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x3F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x8F,0xC7,0xE7,0x67,0x30,0x30,0x30,0x60,0xE0,0x80,0xC0,0xE0,0x60,0x60,0x30,0x30,0x30,0x30,0x30,0x20,0x60,0xC0,0xC0,0xE0,0x60,0x60,0x30,0x30,0x30,0x30,0x30,0x60,0x60,0xC0,0x80,0x80,0xC0,0x60,0x60,0x20,0x33,0x33,0x33,0x33,0x63,0x67,0xC7,0x8F,0x1F,0x33,0xF3,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0x7F,0x0F,0x83,0xE0,0x78,0x1E,0x07,0x01,0x00,0x00,0x80,0xE0,0xF8,0x7E,0x1F,0x07,0x01,0x00,0x00,0x00,0x18,0x18,0x18,0xF8,0xF8,0x1C,0x06,0x03,0x00,0x00,0xC0,0x70,0x3C,0x3C,0x30,0x20,0x20,0xE0,0xF0,0x3D,0x0F,0x03,0x00,0x00,0xC0,0xF0,0xFC,0x3C,0x00,0x00,0x00,0x00,0xC0,0xFF,0x1E,0x00,0xC0,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0x01,0x00,0xFC,0xFF,0x03,0x00,0x00,0x00,0x00,0x00,0x0E,0x0F,0xFF,0xC7,0x00,0x00,0x00,0x00,0x00,0x0E,0x0E,0x0E,0x0E,0xFE,0xC7,0x80,0x00,0x00,0x00,0x00,0x1F,0x1F,0x0E,0x00,0x00,0x80,0xC0,0xF0,0xFF,0x83,0x00,0x00,0x00,0x1C,0x1F,0x0F,0x03,0x00,0x80,0xC0,0xE0,0x78,0x1E,0xC7,0xE1,0xF8,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFE,0xFE,0xFC,0xF3,0xF3,0xE6,0xE6,0xE6,0xE6,0xE6,0xF2,0x83,0x01,0x01,0x03,0x06,0x06,0x06,0x06,0x06,0x06,0x03,0x03,0x01,0x01,0x03,0x03,0x06,0x06,0x06,0x06,0x06,0x06,0x03,0x03,0x01,0x00,0x00,0x01,0x03,0x03,0x06,0x06,0x06,0x06,0x06,0x86,0xF3,0xFB,0xF9,0xFC,0xFE,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xFF,
+ 0x3F,0x40,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0xBF,0x40,0x3F
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_3.txt b/AT91SAM7S256/Source/RCXintro_3.txt
new file mode 100644
index 0000000..eba6710
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_3.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, RCXintro_3) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x02,0x00, // Graphics DataSize
+ 0x10, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x40, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x3F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x07,0x07,0xC3,0xF1,0x79,0x1F,0x00,0x00,0x00,0x00,0xF8,0x7E,0x07,0x03,0x19,0x18,0xF8,0xFC,0xFC,0xFC,0xFF,0xFF,0xFF,0xFF,0xFD,0xFD,0xFC,0xF8,0x31,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x30,0x0C,0x07,0x81,0xE0,0x3C,0x01,0x01,0xC3,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xC3,0x80,0x80,0x80,0x80,0x8F,0xDF,0x70,0x00,0x00,0x00,0x00,0x00,0x7F,0xC0,0x80,0x80,0xCE,0xCE,0x5F,0x7F,0xFF,0xFF,0xBF,0xBF,0x3F,0x3F,0x3F,0x3F,0xBF,0xDF,0x78,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x63,0xC0,0x80,0xC6,0xE3,0xC0,0xE0,0xF8,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0x3F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_4.txt b/AT91SAM7S256/Source/RCXintro_4.txt
new file mode 100644
index 0000000..dc15847
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_4.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, RCXintro_4) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x02,0x00, // Graphics DataSize
+ 0x10, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x40, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x1F,0x3F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0x3F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_5.txt b/AT91SAM7S256/Source/RCXintro_5.txt
new file mode 100644
index 0000000..efd3cb9
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_5.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, RCXintro_5) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x01,0xC0, // Graphics DataSize
+ 0x17, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x34, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0x80,0xC0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xC0,0x80,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xF0,0xE0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xE0,0xF0,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,
+ 0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_6.txt b/AT91SAM7S256/Source/RCXintro_6.txt
new file mode 100644
index 0000000..4ab152a
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_6.txt
@@ -0,0 +1,17 @@
+DEFINE_DATA(BMPMAP, RCXintro_6) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x01,0x20, // Graphics DataSize
+ 0x1C, // Graphics Start X
+ 0x08, // Graphics Start Y
+ 0x2C, // Graphics Width
+ 0x30, // Graphics Height
+BEGIN_DATA
+ 0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x07,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x03,0x07,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x03,0x03,0x03,0x03,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFC,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xFC,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0x07,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_7.txt b/AT91SAM7S256/Source/RCXintro_7.txt
new file mode 100644
index 0000000..cabcc7b
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_7.txt
@@ -0,0 +1,16 @@
+DEFINE_DATA(BMPMAP, RCXintro_7) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0xC8, // Graphics DataSize
+ 0x23, // Graphics Start X
+ 0x10, // Graphics Start Y
+ 0x22, // Graphics Width
+ 0x28, // Graphics Height
+BEGIN_DATA
+ 0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0xC0,0xC0,0xC0,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x0F,0x0F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC,0xF8,0xF8,0xF8,0xF8,0xF8,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0xF0,0xF8,0xF8,0xF8,0xF8,0xF8,0xFC,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0x01,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x01
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_8.txt b/AT91SAM7S256/Source/RCXintro_8.txt
new file mode 100644
index 0000000..d062714
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_8.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, RCXintro_8) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x48, // Graphics DataSize
+ 0x2C, // Graphics Start X
+ 0x18, // Graphics Start Y
+ 0x16, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0xFE,0xFF,0xFF,0xFF,0x1F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x1F,0xFF,0xFF,0xFF,0xFE,
+ 0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x1E,0x1E,0x1E,0x1E,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,
+ 0x1F,0x3F,0x3F,0x3F,0x3E,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x3E,0x3F,0x3F,0x3F,0x1F
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/RCXintro_9.txt b/AT91SAM7S256/Source/RCXintro_9.txt
new file mode 100644
index 0000000..3952437
--- /dev/null
+++ b/AT91SAM7S256/Source/RCXintro_9.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, RCXintro_9) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x30, // Graphics DataSize
+ 0x34, // Graphics Start X
+ 0x18, // Graphics Start Y
+ 0x0E, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,
+ 0xFF,0xFF,0xFF,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0xFF,0xFF,0xFF,
+ 0x07,0x07,0x07,0x07,0x07,0x00,0x00,0x00,0x00,0x07,0x07,0x07,0x07,0x07
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Running.txt b/AT91SAM7S256/Source/Running.txt
new file mode 100644
index 0000000..6bea492
--- /dev/null
+++ b/AT91SAM7S256/Source/Running.txt
@@ -0,0 +1,59 @@
+DEFINE_DATA(ICON, Running) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x04,0x80, // Graphics DataSize
+ 0x01, // Graphics Count X
+ 0x10, // Graphics Count Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x01,0x01,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x1F,0x1F,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x06,0x0E,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xC3,0xC3,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x60,0x70,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x80,0x80,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x80,0x80,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0x70,0x60,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xC3,0xC3,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0x1F,0x1F,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1C,0x0E,0x06,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x01,0x01,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xE0,0xF0,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x38,0xF0,0xE0,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x07,0x0F,0x1C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x18,0x1C,0x0F,0x07,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Status.txt b/AT91SAM7S256/Source/Status.txt
new file mode 100644
index 0000000..6d1d5bd
--- /dev/null
+++ b/AT91SAM7S256/Source/Status.txt
@@ -0,0 +1,17 @@
+DEFINE_DATA(ICON, Status) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x01,0xB0, // Graphics DataSize
+ 0x06, // Graphics Count X
+ 0x06, // Graphics Count Y
+ 0x0C, // Graphics Width
+ 0x08, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x7E,0x81,0x81,0x19,0x19,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x7E,0x01,0x01,0x99,0x99,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x1E,0x81,0x81,0x99,0x99,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x66,0x81,0x81,0x99,0x99,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x78,0x81,0x81,0x99,0x99,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x7E,0x80,0x80,0x99,0x99,0x81,0x81,0x7E,0x00,0x00,
+ 0x00,0x00,0x7E,0x81,0x81,0x98,0x98,0x81,0x81,0x7E,0x00,0x00,0x00,0x00,0x7E,0x81,0x81,0x99,0x99,0x80,0x80,0x7E,0x00,0x00,0x00,0x00,0x7E,0x81,0x81,0x99,0x99,0x81,0x81,0x78,0x00,0x00,0x00,0x00,0x7E,0x81,0x81,0x99,0x99,0x81,0x81,0x66,0x00,0x00,0x00,0x00,0x7E,0x81,0x81,0x99,0x99,0x81,0x81,0x1E,0x00,0x00,0x00,0x00,0x7E,0x81,0x81,0x99,0x99,0x01,0x01,0x7E,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x24,0x24,0x24,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x24,0x24,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x3C,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x3C,0x3C,0x3C,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x24,0x24,0x24,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x24,0x24,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x24,0x24,0x3C,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x18,0x3C,0x3C,0x3C,0x3C,0x3C,0x3C,0x3C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x22,0x14,0x7F,0x2A,0x14,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x22,0x14,0x7F,0x2A,0x14,0x00,0x08,0x14,0x22,0x00,0x00,0x00,0x22,0x14,0x7F,0x2A,0x14,0x00,0x00,0x00,0x22,0x14,0x08,0x00,0x22,0x14,0x7F,0x2A,0x14,0x00,0x08,0x14,0x22,0x14,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3E,0x22,0x1C,0x00,0x3E,0x0A,0x02,0x00,0x3E,0x20,0x3E,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x14,0x14,0x1C,0x08,0x08,0x08,0x08,0x08,0x1C,0x14,0x14,0x00,0x3E,0x20,0x3E,0x00,0x2E,0x2A,0x3A,0x00,0x3E,0x2A,0x3E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Step.txt b/AT91SAM7S256/Source/Step.txt
new file mode 100644
index 0000000..cba0c0d
--- /dev/null
+++ b/AT91SAM7S256/Source/Step.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(ICON, Step) =
+{
+ 0x04,0x00, // Graphics Format
+ 0x02,0xC0, // Graphics DataSize
+ 0x08, // Graphics Count X
+ 0x04, // Graphics Count Y
+ 0x0B, // Graphics Width
+ 0x10, // Graphics Height
+BEGIN_DATA
+ 0xFF,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x11,0x19,0x7D,0x19,0x11,0x01,0x01,0xFF,0xFF,0x01,0x01,0x11,0x19,0x7D,0x19,0x11,0x05,0x01,0xFF,0xFF,0x01,0x11,0x39,0x7D,0x11,0x1D,0x01,0x05,0x01,0xFF,0xFF,0x01,0x11,0x39,0x7D,0x11,0x71,0x01,0x01,0x01,0xFF,0xFF,0x01,0x11,0x39,0x7D,0x11,0x71,0x01,0x05,0x01,0xFF,0xFF,0x01,0x01,0x01,0x1D,0x11,0x7D,0x39,0x11,0x01,0xFF,0xFF,0x01,0x01,0x01,0x71,0x11,0x7D,0x39,0x11,0x01,0xFF,
+ 0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+ 0xFF,0x01,0x05,0x01,0x71,0x11,0x7D,0x39,0x11,0x01,0xFF,0xFF,0x01,0x11,0x39,0x7D,0x11,0x1D,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x21,0x71,0x71,0x3D,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x21,0x71,0x71,0x3D,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x11,0x31,0x7D,0x31,0x11,0x01,0x01,0xFF,0xFF,0x01,0x01,0x11,0x31,0x7D,0x31,0x11,0x05,0x01,0xFF,0xFF,0x01,0x05,0x01,0x1D,0x11,0x7D,0x39,0x11,0x01,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0xFF,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x39,0x45,0x45,0x45,0x39,0x01,0x01,0xFF,0xFF,0x01,0xF1,0x39,0x45,0xFF,0x11,0x45,0x39,0x01,0xFF,0xFF,0x01,0x39,0x29,0x29,0x39,0x45,0x45,0x39,0x01,0xFF,0xFF,0x01,0x01,0x01,0x01,0x01,0x39,0x45,0xFF,0x01,0xFF,0xFF,0x01,0x01,0xC7,0xAB,0x93,0xAB,0xC7,0x01,0x01,0xFF,0xFF,0x01,0x01,0xC7,0xAB,0x93,0xAB,0xC7,0x01,0x01,0xFF,0xFF,0x01,0x01,0xC7,0xAB,0x93,0xAB,0xC7,0x01,0x01,0xFF,
+ 0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+ 0xFF,0x01,0x01,0x39,0x7D,0x7D,0x7D,0x39,0x01,0x01,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x01,0x39,0x41,0x45,0x4F,0x45,0x39,0x01,0x01,0xFF,0xFF,0x01,0x49,0x55,0x25,0x01,0x05,0x7D,0x05,0x01,0xFF,
+ 0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Submenu01.rms b/AT91SAM7S256/Source/Submenu01.rms
new file mode 100644
index 0000000..aadf93d
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu01.rms
@@ -0,0 +1,128 @@
+const UBYTE SUBMENU01[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x01,0xED, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x11, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Software_files
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x10,0x00,0x80,0x01, // 0x10008001
+ 0x06,0x02,0x00,0x01, // 6 ,2 ,0 ,1
+ 0x53,0x6F,0x66,0x74,0x77,0x61,0x72,0x65,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,
+ 0x1C, // 1C
+
+ // NXT_files
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x10,0x00,0x80,0x01, // 0x10008001
+ 0x06,0x03,0x00,0x01, // 6 ,3 ,0 ,1
+ 0x4E,0x58,0x54,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1D, // 1D
+
+ // Sound_files
+ 0x00,0x00,0x00,0x03, // 0x00000003
+ 0x10,0x00,0x80,0x01, // 0x10008001
+ 0x06,0x01,0x00,0x01, // 6 ,1 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,0x00,
+ 0x1B, // 1B
+
+ // Datalog_files
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x80,0x80,0x00, // 0x00808000
+ 0x06,0x05,0x00,0x02, // 6 ,5 ,0 ,2
+ 0x44,0x61,0x74,0x61,0x6C,0x6F,0x67,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,
+ 0x1F, // 1F
+
+ // _
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x06,0xF2,0x00,0x01, // 6 ,242,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // _
+ 0x00,0x00,0x00,0x14, // 0x00000014
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x06,0xF2,0x00,0x01, // 6 ,242,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // Run
+ 0x00,0x00,0x01,0x11, // 0x00000111
+ 0x00,0x00,0x01,0x20, // 0x00000120
+ 0x08,0xF8,0x00,0x00, // 8 ,248,0 ,0
+ 0x52,0x75,0x6E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x32, // 32
+
+ // Send
+ 0x00,0x00,0x02,0x11, // 0x00000211
+ 0x00,0x40,0x00,0x00, // 0x00400000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x53,0x65,0x6E,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x33, // 33
+
+ // Delete
+ 0x00,0x00,0x03,0x11, // 0x00000311
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x02, // 0 ,0 ,0 ,2
+ 0x44,0x65,0x6C,0x65,0x74,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x34, // 34
+
+ // Delete
+ 0x00,0x00,0x01,0x14, // 0x00000114
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x02, // 0 ,0 ,0 ,2
+ 0x44,0x65,0x6C,0x65,0x74,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x34, // 34
+
+ // Send
+ 0x00,0x00,0x02,0x14, // 0x00000214
+ 0x00,0x40,0x00,0x00, // 0x00400000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x53,0x65,0x6E,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x33, // 33
+
+ // _
+ 0x00,0x00,0x12,0x11, // 0x00001211
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x10,0xF9,0x00,0x00, // 16 ,249,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // Are_you_sure?
+ 0x00,0x00,0x13,0x11, // 0x00001311
+ 0x00,0x00,0x00,0x08, // 0x00000008
+ 0x09,0x00,0x00,0x00, // 9 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Are_you_sure?
+ 0x00,0x00,0x23,0x11, // 0x00002311
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x30, // 30
+
+ // Are_you_sure?
+ 0x00,0x00,0x11,0x14, // 0x00001114
+ 0x00,0x00,0x00,0x08, // 0x00000008
+ 0x09,0x00,0x00,0x00, // 9 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Are_you_sure?
+ 0x00,0x00,0x21,0x14, // 0x00002114
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x30, // 30
+
+ // _
+ 0x00,0x00,0x12,0x14, // 0x00001214
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x10,0xF9,0x00,0x00, // 16 ,249,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+};
diff --git a/AT91SAM7S256/Source/Submenu02.rms b/AT91SAM7S256/Source/Submenu02.rms
new file mode 100644
index 0000000..a30478f
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu02.rms
@@ -0,0 +1,401 @@
+const UBYTE SUBMENU02[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x06,0x58, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x38, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // _
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x0B,0xF7,0x00,0x01, // 11 ,247,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // Forward_5
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x23,0x00,0x01, // 11 ,35 ,0 ,1
+ 0x46,0x6F,0x72,0x77,0x61,0x72,0x64,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x23, // 23
+
+ // Forward
+ 0x00,0x00,0x00,0x21, // 0x00000021
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x22,0x00,0x01, // 11 ,34 ,0 ,1
+ 0x46,0x6F,0x72,0x77,0x61,0x72,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x22, // 22
+
+ // Turn_right_2
+ 0x00,0x00,0x00,0x31, // 0x00000031
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x29,0x00,0x01, // 11 ,41 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x72,0x69,0x67,0x68,0x74,0x20,0x32,0x00,0x00,0x00,0x00,
+ 0x29, // 29
+
+ // Turn_right
+ 0x00,0x00,0x00,0x41, // 0x00000041
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x28,0x00,0x01, // 11 ,40 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x72,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x28, // 28
+
+ // Back_right_2
+ 0x00,0x00,0x00,0x51, // 0x00000051
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2F,0x00,0x01, // 11 ,47 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x72,0x69,0x67,0x68,0x74,0x20,0x32,0x00,0x00,0x00,0x00,
+ 0x2F, // 2F
+
+ // Back_right
+ 0x00,0x00,0x00,0x61, // 0x00000061
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x27,0x00,0x01, // 11 ,39 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x72,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x27, // 27
+
+ // Tone_1
+ 0x00,0x00,0x00,0x71, // 0x00000071
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2B,0x00,0x01, // 11 ,43 ,0 ,1
+ 0x54,0x6F,0x6E,0x65,0x20,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2B, // 2B
+
+ // Tone_2
+ 0x00,0x00,0x00,0x81, // 0x00000081
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2C,0x00,0x01, // 11 ,44 ,0 ,1
+ 0x54,0x6F,0x6E,0x65,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2C, // 2C
+
+ // Back_left_2
+ 0x00,0x00,0x00,0x91, // 0x00000091
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x24,0x00,0x01, // 11 ,36 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x6C,0x65,0x66,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,
+ 0x24, // 24
+
+ // Back_left
+ 0x00,0x00,0x00,0xA1, // 0x000000A1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2A,0x00,0x01, // 11 ,42 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x6C,0x65,0x66,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2A, // 2A
+
+ // Turn_left
+ 0x00,0x00,0x00,0xB1, // 0x000000B1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x25,0x00,0x01, // 11 ,37 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6C,0x65,0x66,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x25, // 25
+
+ // Turn_left_2
+ 0x00,0x00,0x00,0xC1, // 0x000000C1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x26,0x00,0x01, // 11 ,38 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6C,0x65,0x66,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,
+ 0x26, // 26
+
+ // Empty
+ 0x00,0x00,0x00,0xD1, // 0x000000D1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x21,0x00,0x01, // 11 ,33 ,0 ,1
+ 0x45,0x6D,0x70,0x74,0x79,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x21, // 21
+
+ // Backward
+ 0x00,0x00,0x00,0xE1, // 0x000000E1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2D,0x00,0x01, // 11 ,45 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x77,0x61,0x72,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2D, // 2D
+
+ // Backward_5
+ 0x00,0x00,0x00,0xF1, // 0x000000F1
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2E,0x00,0x01, // 11 ,46 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x77,0x61,0x72,0x64,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2E, // 2E
+
+ // Empty
+ 0x00,0x00,0x01,0x11, // 0x00000111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x41,0x00,0x01, // 11 ,65 ,0 ,1
+ 0x45,0x6D,0x70,0x74,0x79,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x21, // 21
+
+ // Wait_2
+ 0x00,0x00,0x02,0x11, // 0x00000211
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x46,0x00,0x01, // 11 ,70 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x46, // 46
+
+ // Wait_5
+ 0x00,0x00,0x03,0x11, // 0x00000311
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x47,0x00,0x01, // 11 ,71 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x47, // 47
+
+ // Wait_10
+ 0x00,0x00,0x04,0x11, // 0x00000411
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x48,0x00,0x01, // 11 ,72 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x31,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x48, // 48
+
+ // Object
+ 0x00,0x00,0x05,0x11, // 0x00000511
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x43,0x00,0x01, // 11 ,67 ,0 ,1
+ 0x4F,0x62,0x6A,0x65,0x63,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x43, // 43
+
+ // Sound
+ 0x00,0x00,0x06,0x11, // 0x00000611
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x44,0x00,0x01, // 11 ,68 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x44, // 44
+
+ // Light
+ 0x00,0x00,0x07,0x11, // 0x00000711
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x42,0x00,0x01, // 11 ,66 ,0 ,1
+ 0x4C,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x42, // 42
+
+ // Dark
+ 0x00,0x00,0x08,0x11, // 0x00000811
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x49,0x00,0x01, // 11 ,73 ,0 ,1
+ 0x44,0x61,0x72,0x6B,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x49, // 49
+
+ // Touch
+ 0x00,0x00,0x09,0x11, // 0x00000911
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x45,0x00,0x01, // 11 ,69 ,0 ,1
+ 0x54,0x6F,0x75,0x63,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x45, // 45
+
+ // Forward_5
+ 0x00,0x00,0x11,0x11, // 0x00001111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x23,0x00,0x01, // 11 ,35 ,0 ,1
+ 0x46,0x6F,0x72,0x77,0x61,0x72,0x64,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x23, // 23
+
+ // Forward
+ 0x00,0x00,0x21,0x11, // 0x00002111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x22,0x00,0x01, // 11 ,34 ,0 ,1
+ 0x46,0x6F,0x72,0x77,0x61,0x72,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x22, // 22
+
+ // Turn_right_2
+ 0x00,0x00,0x31,0x11, // 0x00003111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x29,0x00,0x01, // 11 ,41 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x72,0x69,0x67,0x68,0x74,0x20,0x32,0x00,0x00,0x00,0x00,
+ 0x29, // 29
+
+ // Turn_right
+ 0x00,0x00,0x41,0x11, // 0x00004111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x28,0x00,0x01, // 11 ,40 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x72,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x28, // 28
+
+ // Back_right_2
+ 0x00,0x00,0x51,0x11, // 0x00005111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2F,0x00,0x01, // 11 ,47 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x72,0x69,0x67,0x68,0x74,0x20,0x32,0x00,0x00,0x00,0x00,
+ 0x2F, // 2F
+
+ // Back_right
+ 0x00,0x00,0x61,0x11, // 0x00006111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x27,0x00,0x01, // 11 ,39 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x72,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x27, // 27
+
+ // Tone_1
+ 0x00,0x00,0x71,0x11, // 0x00007111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2B,0x00,0x01, // 11 ,43 ,0 ,1
+ 0x54,0x6F,0x6E,0x65,0x20,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2B, // 2B
+
+ // Tone_2
+ 0x00,0x00,0x81,0x11, // 0x00008111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2C,0x00,0x01, // 11 ,44 ,0 ,1
+ 0x54,0x6F,0x6E,0x65,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2C, // 2C
+
+ // Back_left_2
+ 0x00,0x00,0x91,0x11, // 0x00009111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x24,0x00,0x01, // 11 ,36 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x6C,0x65,0x66,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,
+ 0x24, // 24
+
+ // Back_left
+ 0x00,0x00,0xA1,0x11, // 0x0000A111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2A,0x00,0x01, // 11 ,42 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x20,0x6C,0x65,0x66,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2A, // 2A
+
+ // Turn_left
+ 0x00,0x00,0xB1,0x11, // 0x0000B111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x25,0x00,0x01, // 11 ,37 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6C,0x65,0x66,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x25, // 25
+
+ // Turn_left_2
+ 0x00,0x00,0xC1,0x11, // 0x0000C111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x26,0x00,0x01, // 11 ,38 ,0 ,1
+ 0x54,0x75,0x72,0x6E,0x20,0x6C,0x65,0x66,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,
+ 0x26, // 26
+
+ // Empty
+ 0x00,0x00,0xD1,0x11, // 0x0000D111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x21,0x00,0x01, // 11 ,33 ,0 ,1
+ 0x45,0x6D,0x70,0x74,0x79,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x21, // 21
+
+ // Backward
+ 0x00,0x00,0xE1,0x11, // 0x0000E111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2D,0x00,0x01, // 11 ,45 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x77,0x61,0x72,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2D, // 2D
+
+ // Backward_5
+ 0x00,0x00,0xF1,0x11, // 0x0000F111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x2E,0x00,0x01, // 11 ,46 ,0 ,1
+ 0x42,0x61,0x63,0x6B,0x77,0x61,0x72,0x64,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x2E, // 2E
+
+ // Empty
+ 0x00,0x01,0x11,0x11, // 0x00011111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x41,0x00,0x01, // 11 ,65 ,0 ,1
+ 0x45,0x6D,0x70,0x74,0x79,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x21, // 21
+
+ // Wait_2
+ 0x00,0x02,0x11,0x11, // 0x00021111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x46,0x00,0x01, // 11 ,70 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x46, // 46
+
+ // Wait_5
+ 0x00,0x03,0x11,0x11, // 0x00031111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x47,0x00,0x01, // 11 ,71 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x35,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x47, // 47
+
+ // Wait_10
+ 0x00,0x04,0x11,0x11, // 0x00041111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x48,0x00,0x01, // 11 ,72 ,0 ,1
+ 0x57,0x61,0x69,0x74,0x20,0x31,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x48, // 48
+
+ // Object
+ 0x00,0x05,0x11,0x11, // 0x00051111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x43,0x00,0x01, // 11 ,67 ,0 ,1
+ 0x4F,0x62,0x6A,0x65,0x63,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x43, // 43
+
+ // Sound
+ 0x00,0x06,0x11,0x11, // 0x00061111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x44,0x00,0x01, // 11 ,68 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x44, // 44
+
+ // Light
+ 0x00,0x07,0x11,0x11, // 0x00071111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x42,0x00,0x01, // 11 ,66 ,0 ,1
+ 0x4C,0x69,0x67,0x68,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x42, // 42
+
+ // Dark
+ 0x00,0x08,0x11,0x11, // 0x00081111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x49,0x00,0x01, // 11 ,73 ,0 ,1
+ 0x44,0x61,0x72,0x6B,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x49, // 49
+
+ // Touch
+ 0x00,0x09,0x11,0x11, // 0x00091111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0x45,0x00,0x01, // 11 ,69 ,0 ,1
+ 0x54,0x6F,0x75,0x63,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x45, // 45
+
+ // Stop
+ 0x00,0x11,0x11,0x11, // 0x00111111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0xFB,0x00,0x01, // 11 ,251,0 ,1
+ 0x53,0x74,0x6F,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x4D, // 4D
+
+ // Loop
+ 0x00,0x21,0x11,0x11, // 0x00211111
+ 0x10,0x00,0x00,0x61, // 0x10000061
+ 0x0B,0xFC,0x00,0x01, // 11 ,252,0 ,1
+ 0x4C,0x6F,0x6F,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x4E, // 4E
+
+ // Run
+ 0x01,0x11,0x11,0x11, // 0x01111111
+ 0x00,0x00,0x00,0x60, // 0x00000060
+ 0x0B,0xF8,0x00,0x00, // 11 ,248,0 ,0
+ 0x52,0x75,0x6E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x32, // 32
+
+ // Main_menu
+ 0x02,0x11,0x11,0x11, // 0x02111111
+ 0x00,0x00,0x20,0x60, // 0x00002060
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x4D,0x61,0x69,0x6E,0x20,0x6D,0x65,0x6E,0x75,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x37, // 37
+
+ // Save
+ 0x04,0x11,0x11,0x11, // 0x04111111
+ 0x00,0x00,0x00,0x60, // 0x00000060
+ 0x0B,0xFA,0x00,0x02, // 11 ,250,0 ,2
+ 0x53,0x61,0x76,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1D, // 1D
+
+ // Yes
+ 0x14,0x11,0x11,0x11, // 0x14111111
+ 0x00,0x00,0x20,0x20, // 0x00002020
+ 0x0B,0xED,0x00,0x00, // 11 ,237,0 ,0
+ 0x59,0x65,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // No
+ 0x24,0x11,0x11,0x11, // 0x24111111
+ 0x00,0x08,0x00,0x24, // 0x00080024
+ 0x0B,0xF6,0x00,0x00, // 11 ,246,0 ,0
+ 0x4E,0x6F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x30, // 30
+};
diff --git a/AT91SAM7S256/Source/Submenu03.rms b/AT91SAM7S256/Source/Submenu03.rms
new file mode 100644
index 0000000..cb7830b
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu03.rms
@@ -0,0 +1,233 @@
+const UBYTE SUBMENU03[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x03,0xA0, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x20, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Temperature_`C
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x0B,0x00,0x01, // 10 ,11 ,0 ,1
+ 0x54,0x65,0x6D,0x70,0x65,0x72,0x61,0x74,0x75,0x72,0x65,0x20,0x60,0x43,0x00,0x00,
+ 0x0F, // 0F
+
+ // Temperature_`F
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x0C,0x00,0x01, // 10 ,12 ,0 ,1
+ 0x54,0x65,0x6D,0x70,0x65,0x72,0x61,0x74,0x75,0x72,0x65,0x20,0x60,0x46,0x00,0x00,
+ 0x10, // 10
+
+ // Sound_dB
+ 0x00,0x00,0x00,0x03, // 0x00000003
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x02,0x00,0x01, // 10 ,2 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x64,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x02, // 02
+
+ // Sound_dBA
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x03,0x00,0x01, // 10 ,3 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x64,0x42,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x03, // 03
+
+ // Reflected_light
+ 0x00,0x00,0x00,0x05, // 0x00000005
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x04,0x00,0x01, // 10 ,4 ,0 ,1
+ 0x52,0x65,0x66,0x6C,0x65,0x63,0x74,0x65,0x64,0x20,0x6C,0x69,0x67,0x68,0x74,0x00,
+ 0x04, // 04
+
+ // Ambient_light
+ 0x00,0x00,0x00,0x06, // 0x00000006
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x05,0x00,0x01, // 10 ,5 ,0 ,1
+ 0x41,0x6D,0x62,0x69,0x65,0x6E,0x74,0x20,0x6C,0x69,0x67,0x68,0x74,0x00,0x00,0x00,
+ 0x05, // 05
+
+ // Motor_Rotations
+ 0x00,0x00,0x00,0x07, // 0x00000007
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0A,0x08,0x00,0x01, // 10 ,8 ,0 ,1
+ 0x4D,0x6F,0x74,0x6F,0x72,0x20,0x52,0x6F,0x74,0x61,0x74,0x69,0x6F,0x6E,0x73,0x00,
+ 0x09, // 09
+
+ // Motor_Degrees
+ 0x00,0x00,0x00,0x08, // 0x00000008
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0A,0x07,0x00,0x01, // 10 ,7 ,0 ,1
+ 0x4D,0x6F,0x74,0x6F,0x72,0x20,0x44,0x65,0x67,0x72,0x65,0x65,0x73,0x00,0x00,0x00,
+ 0x08, // 08
+
+ // Touch
+ 0x00,0x00,0x00,0x09, // 0x00000009
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x06,0x00,0x01, // 10 ,6 ,0 ,1
+ 0x54,0x6F,0x75,0x63,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x07, // 07
+
+ // UltraSonic_inch
+ 0x00,0x00,0x00,0x0A, // 0x0000000A
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x09,0x00,0x01, // 10 ,9 ,0 ,1
+ 0x55,0x6C,0x74,0x72,0x61,0x53,0x6F,0x6E,0x69,0x63,0x20,0x69,0x6E,0x63,0x68,0x00,
+ 0x0B, // 0B
+
+ // UltraSonic_cm
+ 0x00,0x00,0x00,0x0B, // 0x0000000B
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x0A,0x00,0x01, // 10 ,10 ,0 ,1
+ 0x55,0x6C,0x74,0x72,0x61,0x53,0x6F,0x6E,0x69,0x63,0x20,0x63,0x6D,0x00,0x00,0x00,
+ 0x0C, // 0C
+
+ // Color
+ 0x00,0x00,0x00,0x0C, // 0x0000000C
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x0D,0x00,0x01, // 10 ,13 ,0 ,1
+ 0x43,0x6F,0x6C,0x6F,0x72,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x11, // 11
+
+ // Done
+ 0x00,0x00,0x00,0x0D, // 0x0000000D
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0A,0xEE,0x00,0x01, // 10 ,238,0 ,1
+ 0x44,0x6F,0x6E,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Port_1
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x12,0x00,0x01, // 10 ,18 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x12, // 12
+
+ // Port_2
+ 0x00,0x00,0x00,0x21, // 0x00000021
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x13,0x00,0x01, // 10 ,19 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x13, // 13
+
+ // Port_3
+ 0x00,0x00,0x00,0x31, // 0x00000031
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x14,0x00,0x01, // 10 ,20 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x14, // 14
+
+ // Port_4
+ 0x00,0x00,0x00,0x41, // 0x00000041
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x15,0x00,0x01, // 10 ,21 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x34,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x15, // 15
+
+ // Port_A
+ 0x00,0x00,0x00,0x17, // 0x00000017
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x16,0x00,0x01, // 10 ,22 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x16, // 16
+
+ // Port_B
+ 0x00,0x00,0x00,0x27, // 0x00000027
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x17,0x00,0x01, // 10 ,23 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x17, // 17
+
+ // Port_C
+ 0x00,0x00,0x00,0x37, // 0x00000037
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x18,0x00,0x01, // 10 ,24 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x18, // 18
+
+ // Port_A
+ 0x00,0x00,0x00,0x18, // 0x00000018
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x16,0x00,0x01, // 10 ,22 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x16, // 16
+
+ // Port_B
+ 0x00,0x00,0x00,0x28, // 0x00000028
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x17,0x00,0x01, // 10 ,23 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x17, // 17
+
+ // Port_C
+ 0x00,0x00,0x00,0x38, // 0x00000038
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0A,0x18,0x00,0x01, // 10 ,24 ,0 ,1
+ 0x50,0x6F,0x72,0x74,0x20,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x17, // 17
+
+ // _
+ 0x00,0x00,0x00,0x1D, // 0x0000001D
+ 0x00,0x00,0x10,0x00, // 0x00001000
+ 0x0A,0xF7,0x00,0x01, // 10 ,247,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // _
+ 0x00,0x00,0x01,0x11, // 0x00000111
+ 0x0D,0x05,0x10,0x00, // 0x0D051000
+ 0x0A,0xF2,0x00,0x00, // 10 ,242,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // _
+ 0x00,0x00,0x01,0x17, // 0x00000117
+ 0x0D,0x05,0x10,0x00, // 0x0D051000
+ 0x0A,0xF2,0x00,0x00, // 10 ,242,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // _
+ 0x00,0x00,0x01,0x18, // 0x00000118
+ 0x0D,0x05,0x10,0x00, // 0x0D051000
+ 0x0A,0xF2,0x00,0x00, // 10 ,242,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00, // 00
+
+ // Run
+ 0x00,0x00,0x01,0x1D, // 0x0000011D
+ 0x00,0x00,0x00,0x68, // 0x00000068
+ 0x0A,0xF8,0x00,0x02, // 10 ,248,0 ,2
+ 0x52,0x75,0x6E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x32, // 32
+
+ // Main_menu
+ 0x00,0x00,0x11,0x1D, // 0x0000111D
+ 0x00,0x02,0x20,0x00, // 0x00022000
+ 0x0A,0xF1,0x00,0x00, // 10 ,241,0 ,0
+ 0x4D,0x61,0x69,0x6E,0x20,0x6D,0x65,0x6E,0x75,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x37, // 37
+
+ // Save
+ 0x00,0x00,0x21,0x1D, // 0x0000211D
+ 0x00,0x02,0x00,0x00, // 0x00020000
+ 0x0A,0xFA,0x00,0x02, // 10 ,250,0 ,2
+ 0x53,0x61,0x76,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1F, // 1F
+
+ // Yes
+ 0x00,0x01,0x21,0x1D, // 0x0001211D
+ 0x00,0x00,0x20,0x20, // 0x00002020
+ 0x0A,0xED,0x00,0x00, // 10 ,237,0 ,0
+ 0x59,0x65,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // No
+ 0x00,0x02,0x21,0x1D, // 0x0002211D
+ 0x00,0x08,0x00,0x24, // 0x00080024
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x4E,0x6F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x30, // 30
+};
diff --git a/AT91SAM7S256/Source/Submenu04.rms b/AT91SAM7S256/Source/Submenu04.rms
new file mode 100644
index 0000000..273e456
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu04.rms
@@ -0,0 +1,163 @@
+const UBYTE SUBMENU04[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x02,0x7E, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x16, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Sound_dB
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x02,0x00,0x01, // 14 ,2 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x64,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x02, // 02
+
+ // Sound_dBA
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x03,0x00,0x01, // 14 ,3 ,0 ,1
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x64,0x42,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x03, // 03
+
+ // Reflected_light
+ 0x00,0x00,0x00,0x03, // 0x00000003
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x04,0x00,0x01, // 14 ,4 ,0 ,1
+ 0x52,0x65,0x66,0x6C,0x65,0x63,0x74,0x65,0x64,0x20,0x6C,0x69,0x67,0x68,0x74,0x00,
+ 0x04, // 04
+
+ // Ambient_light
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x05,0x00,0x01, // 14 ,5 ,0 ,1
+ 0x41,0x6D,0x62,0x69,0x65,0x6E,0x74,0x20,0x6C,0x69,0x67,0x68,0x74,0x00,0x00,0x00,
+ 0x05, // 05
+
+ // Temperature_`C
+ 0x00,0x00,0x00,0x05, // 0x00000005
+ 0x10,0x00,0x01,0x21, // 0x10000121
+ 0x0E,0x0B,0x00,0x01, // 14 ,11 ,0 ,1
+ 0x54,0x65,0x6D,0x70,0x65,0x72,0x61,0x74,0x75,0x72,0x65,0x20,0x60,0x43,0x00,0x00,
+ 0x0F, // 0F
+
+ // Temperature_`F
+ 0x00,0x00,0x00,0x06, // 0x00000006
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x0C,0x00,0x01, // 14 ,12 ,0 ,1
+ 0x54,0x65,0x6D,0x70,0x65,0x72,0x61,0x74,0x75,0x72,0x65,0x20,0x60,0x46,0x00,0x00,
+ 0x10, // 10
+
+ // Motor_rotations
+ 0x00,0x00,0x00,0x07, // 0x00000007
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x08,0x00,0x01, // 14 ,8 ,0 ,1
+ 0x4D,0x6F,0x74,0x6F,0x72,0x20,0x72,0x6F,0x74,0x61,0x74,0x69,0x6F,0x6E,0x73,0x00,
+ 0x09, // 09
+
+ // Motor_degrees
+ 0x00,0x00,0x00,0x08, // 0x00000008
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x07,0x00,0x01, // 14 ,7 ,0 ,1
+ 0x4D,0x6F,0x74,0x6F,0x72,0x20,0x64,0x65,0x67,0x72,0x65,0x65,0x73,0x00,0x00,0x00,
+ 0x08, // 08
+
+ // Touch
+ 0x00,0x00,0x00,0x09, // 0x00000009
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x06,0x00,0x01, // 14 ,6 ,0 ,1
+ 0x54,0x6F,0x75,0x63,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x07, // 07
+
+ // Ultrasonic_inch
+ 0x00,0x00,0x00,0x0A, // 0x0000000A
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x09,0x00,0x01, // 14 ,9 ,0 ,1
+ 0x55,0x6C,0x74,0x72,0x61,0x73,0x6F,0x6E,0x69,0x63,0x20,0x69,0x6E,0x63,0x68,0x00,
+ 0x0B, // 0B
+
+ // Ultrasonic_cm
+ 0x00,0x00,0x00,0x0B, // 0x0000000B
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x0A,0x00,0x01, // 14 ,10 ,0 ,1
+ 0x55,0x6C,0x74,0x72,0x61,0x73,0x6F,0x6E,0x69,0x63,0x20,0x63,0x6D,0x00,0x00,0x00,
+ 0x0C, // 0C
+
+ // Color
+ 0x00,0x00,0x00,0x0C, // 0x0000000C
+ 0x10,0x00,0x00,0x21, // 0x10000021
+ 0x0E,0x0D,0x00,0x01, // 14 ,13 ,0 ,1
+ 0x43,0x6F,0x6C,0x6F,0x72,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x11, // 11
+
+ // Port_1
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x12,0x00,0x00, // 14 ,18 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x12, // 12
+
+ // Port_2
+ 0x00,0x00,0x00,0x21, // 0x00000021
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x13,0x00,0x00, // 14 ,19 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x13, // 13
+
+ // Port_3
+ 0x00,0x00,0x00,0x31, // 0x00000031
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x14,0x00,0x00, // 14 ,20 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x14, // 14
+
+ // Port_4
+ 0x00,0x00,0x00,0x41, // 0x00000041
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x15,0x00,0x00, // 14 ,21 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x34,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x15, // 15
+
+ // Port_A
+ 0x00,0x00,0x00,0x17, // 0x00000017
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x16,0x00,0x00, // 14 ,22 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x16, // 16
+
+ // Port_B
+ 0x00,0x00,0x00,0x27, // 0x00000027
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x17,0x00,0x00, // 14 ,23 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x17, // 17
+
+ // Port_C
+ 0x00,0x00,0x00,0x37, // 0x00000037
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x18,0x00,0x00, // 14 ,24 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x18, // 18
+
+ // Port_A
+ 0x00,0x00,0x00,0x18, // 0x00000018
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x16,0x00,0x00, // 14 ,22 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x16, // 16
+
+ // Port_B
+ 0x00,0x00,0x00,0x28, // 0x00000028
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x17,0x00,0x00, // 14 ,23 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x17, // 17
+
+ // Port_C
+ 0x00,0x00,0x00,0x38, // 0x00000038
+ 0x00,0x00,0x00,0x20, // 0x00000020
+ 0x0E,0x18,0x00,0x00, // 14 ,24 ,0 ,0
+ 0x50,0x6F,0x72,0x74,0x20,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x18, // 18
+};
diff --git a/AT91SAM7S256/Source/Submenu05.rms b/AT91SAM7S256/Source/Submenu05.rms
new file mode 100644
index 0000000..5e03396
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu05.rms
@@ -0,0 +1,128 @@
+const UBYTE SUBMENU05[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x01,0xED, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x11, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Volume
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x00,0x00,0x80,0x00, // 0x00008000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x56,0x6F,0x6C,0x75,0x6D,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x39, // 39
+
+ // Sleep
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x00,0x00,0x80,0x00, // 0x00008000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x53,0x6C,0x65,0x65,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x3A, // 3A
+
+ // NXT_Version
+ 0x00,0x00,0x00,0x03, // 0x00000003
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x01,0x00,0x00,0x00, // 1 ,0 ,0 ,0
+ 0x4E,0x58,0x54,0x20,0x56,0x65,0x72,0x73,0x69,0x6F,0x6E,0x00,0x00,0x00,0x00,0x00,
+ 0x4F, // 4F
+
+ // Delete_files
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x00,0x80,0x00, // 0x00008000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x44,0x65,0x6C,0x65,0x74,0x65,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,
+ 0x34, // 34
+
+ // _
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x00,0x00,0x03,0x60, // 0x00000360
+ 0x07,0xEF,0x00,0x00, // 7 ,239,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x39, // 39
+
+ // _
+ 0x00,0x00,0x00,0x21, // 0x00000021
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x35, // 35
+
+ // _
+ 0x00,0x00,0x00,0x31, // 0x00000031
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x36, // 36
+
+ // _
+ 0x00,0x00,0x00,0x12, // 0x00000012
+ 0x00,0x00,0x03,0x20, // 0x00000320
+ 0x04,0xEF,0x00,0x00, // 4 ,239,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x3A, // 3A
+
+ // _
+ 0x00,0x00,0x00,0x22, // 0x00000022
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x35, // 35
+
+ // _
+ 0x00,0x00,0x00,0x32, // 0x00000032
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x36, // 36
+
+ // Software_files
+ 0x00,0x00,0x00,0x14, // 0x00000014
+ 0x10,0x00,0x00,0x01, // 0x10000001
+ 0x05,0x02,0x00,0x02, // 5 ,2 ,0 ,2
+ 0x53,0x6F,0x66,0x74,0x77,0x61,0x72,0x65,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,
+ 0x1C, // 1C
+
+ // NXT_files
+ 0x00,0x00,0x00,0x24, // 0x00000024
+ 0x10,0x00,0x00,0x01, // 0x10000001
+ 0x05,0x03,0x00,0x02, // 5 ,3 ,0 ,2
+ 0x4E,0x58,0x54,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1D, // 1D
+
+ // Sound_files
+ 0x00,0x00,0x00,0x34, // 0x00000034
+ 0x10,0x00,0x00,0x01, // 0x10000001
+ 0x05,0x01,0x00,0x02, // 5 ,1 ,0 ,2
+ 0x53,0x6F,0x75,0x6E,0x64,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,0x00,
+ 0x1B, // 1B
+
+ // Datalog_files
+ 0x00,0x00,0x00,0x44, // 0x00000044
+ 0x10,0x80,0x00,0x01, // 0x10800001
+ 0x05,0x05,0x00,0x02, // 5 ,5 ,0 ,2
+ 0x44,0x61,0x74,0x61,0x6C,0x6F,0x67,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,
+ 0x1F, // 1F
+
+ // Try_me_files
+ 0x00,0x00,0x00,0x54, // 0x00000054
+ 0x10,0x00,0x00,0x01, // 0x10000001
+ 0x05,0x04,0x00,0x02, // 5 ,4 ,0 ,2
+ 0x54,0x72,0x79,0x20,0x6D,0x65,0x20,0x66,0x69,0x6C,0x65,0x73,0x00,0x00,0x00,0x00,
+ 0x1E, // 1E
+
+ // Are_you_sure?
+ 0x00,0x00,0x01,0x14, // 0x00000114
+ 0x00,0x00,0x01,0x08, // 0x00000108
+ 0x05,0xF1,0x00,0x00, // 5 ,241,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Are_you_sure?
+ 0x00,0x00,0x02,0x14, // 0x00000214
+ 0x00,0x00,0x01,0x04, // 0x00000104
+ 0x05,0x00,0x00,0x00, // 5 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x30, // 30
+};
diff --git a/AT91SAM7S256/Source/Submenu06.rms b/AT91SAM7S256/Source/Submenu06.rms
new file mode 100644
index 0000000..5cd1b06
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu06.rms
@@ -0,0 +1,51 @@
+const UBYTE SUBMENU06[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x00,0xAE, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x06, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // _
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x00,0x00,0x10,0x00, // 0x00001000
+ 0x06,0x04,0x00,0x01, // 6 ,4 ,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1E, // 1E
+
+ // _
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x00,0x00,0x03,0x80, // 0x00000380
+ 0x06,0xF2,0x00,0x02, // 6 ,242,0 ,2
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x1E, // 1E
+
+ // Delete
+ 0x00,0x00,0x01,0x11, // 0x00000111
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x02, // 0 ,0 ,0 ,2
+ 0x44,0x65,0x6C,0x65,0x74,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x34, // 34
+
+ // Run
+ 0x00,0x00,0x02,0x11, // 0x00000211
+ 0x00,0x00,0x01,0x20, // 0x00000120
+ 0x08,0xF8,0x00,0x00, // 8 ,248,0 ,0
+ 0x52,0x75,0x6E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x32, // 32
+
+ // Are_you_sure?
+ 0x00,0x00,0x11,0x11, // 0x00001111
+ 0x00,0x00,0x00,0x08, // 0x00000008
+ 0x09,0x00,0x00,0x00, // 9 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x31, // 31
+
+ // Are_you_sure?
+ 0x00,0x00,0x21,0x11, // 0x00002111
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x00,0x00,0x00, // 0 ,0 ,0 ,0
+ 0x41,0x72,0x65,0x20,0x79,0x6F,0x75,0x20,0x73,0x75,0x72,0x65,0x3F,0x00,0x00,0x00,
+ 0x30, // 30
+};
diff --git a/AT91SAM7S256/Source/Submenu07.rms b/AT91SAM7S256/Source/Submenu07.rms
new file mode 100644
index 0000000..43c292e
--- /dev/null
+++ b/AT91SAM7S256/Source/Submenu07.rms
@@ -0,0 +1,142 @@
+const UBYTE SUBMENU07[] =
+{
+ 0x07,0x00, // Menu Format
+ 0x02,0x27, // Menu DataSize
+ 0x1D, // Menu item size
+ 0x13, // Menu items
+ 0x18, // Menu icon Width
+ 0x18, // Menu icon Height
+
+ // Search
+ 0x00,0x00,0x00,0x01, // 0x00000001
+ 0x00,0x40,0x80,0x00, // 0x00408000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x53,0x65,0x61,0x72,0x63,0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x56, // 56
+
+ // My_contacts
+ 0x00,0x00,0x00,0x02, // 0x00000002
+ 0x00,0x40,0x80,0x00, // 0x00408000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x4D,0x79,0x20,0x63,0x6F,0x6E,0x74,0x61,0x63,0x74,0x73,0x00,0x00,0x00,0x00,0x00,
+ 0x52, // 52
+
+ // Connections
+ 0x00,0x00,0x00,0x03, // 0x00000003
+ 0x00,0x40,0x80,0x00, // 0x00408000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x43,0x6F,0x6E,0x6E,0x65,0x63,0x74,0x69,0x6F,0x6E,0x73,0x00,0x00,0x00,0x00,0x00,
+ 0x53, // 53
+
+ // Visibility
+ 0x00,0x00,0x00,0x04, // 0x00000004
+ 0x00,0x40,0x80,0x00, // 0x00408000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x56,0x69,0x73,0x69,0x62,0x69,0x6C,0x69,0x74,0x79,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x54, // 54
+
+ // On/Off
+ 0x00,0x00,0x00,0x05, // 0x00000005
+ 0x00,0x00,0x80,0x00, // 0x00008000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x4F,0x6E,0x2F,0x4F,0x66,0x66,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x55, // 55
+
+ // _
+ 0x00,0x00,0x00,0x11, // 0x00000011
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x12,0xFF,0x00,0x01, // 18 ,255,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // _
+ 0x00,0x00,0x00,0x12, // 0x00000012
+ 0x00,0x00,0x10,0x00, // 0x00001000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // _
+ 0x00,0x00,0x00,0x13, // 0x00000013
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x14,0xF6,0x00,0x01, // 20 ,246,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // Visible
+ 0x00,0x00,0x00,0x14, // 0x00000014
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x11,0xEB,0x00,0x00, // 17 ,235,0 ,0
+ 0x56,0x69,0x73,0x69,0x62,0x6C,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x5A, // 5A
+
+ // Invisible
+ 0x00,0x00,0x00,0x24, // 0x00000024
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x11,0xEA,0x00,0x00, // 17 ,234,0 ,0
+ 0x49,0x6E,0x76,0x69,0x73,0x69,0x62,0x6C,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x5B, // 5B
+
+ // On
+ 0x00,0x00,0x00,0x15, // 0x00000015
+ 0x00,0x00,0x00,0x80, // 0x00000080
+ 0x03,0xEB,0x00,0x00, // 3 ,235,0 ,0
+ 0x4F,0x6E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x5C, // 5C
+
+ // Off
+ 0x00,0x00,0x00,0x25, // 0x00000025
+ 0x00,0x40,0x00,0x80, // 0x00400080
+ 0x03,0xEA,0x00,0x00, // 3 ,234,0 ,0
+ 0x4F,0x66,0x66,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x5D, // 5D
+
+ // _
+ 0x00,0x00,0x01,0x11, // 0x00000111
+ 0x00,0x00,0x03,0x08, // 0x00000308
+ 0x13,0xF2,0x00,0x01, // 19 ,242,0 ,1
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // _
+ 0x00,0x00,0x01,0x12, // 0x00000112
+ 0x00,0x10,0x02,0x08, // 0x00100208
+ 0x13,0xF2,0x00,0x02, // 19 ,242,0 ,2
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // Disconnect
+ 0x00,0x00,0x01,0x13, // 0x00000113
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x14,0xF0,0x00,0x00, // 20 ,240,0 ,0
+ 0x44,0x69,0x73,0x63,0x6F,0x6E,0x6E,0x65,0x63,0x74,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x59, // 59
+
+ // _
+ 0x00,0x00,0x11,0x11, // 0x00001111
+ 0x00,0x00,0x03,0x00, // 0x00000300
+ 0x10,0xF5,0x00,0x00, // 16 ,245,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+
+ // Delete
+ 0x00,0x00,0x11,0x12, // 0x00001112
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x13,0xF1,0x00,0x00, // 19 ,241,0 ,0
+ 0x44,0x65,0x6C,0x65,0x74,0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x34, // 34
+
+ // Connect
+ 0x00,0x00,0x21,0x12, // 0x00002112
+ 0x00,0x00,0x00,0x00, // 0x00000000
+ 0x00,0x00,0x00,0x01, // 0 ,0 ,0 ,1
+ 0x43,0x6F,0x6E,0x6E,0x65,0x63,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x58, // 58
+
+ // _
+ 0x00,0x01,0x21,0x12, // 0x00012112
+ 0x00,0x00,0x03,0x08, // 0x00000308
+ 0x10,0xF5,0x00,0x00, // 16 ,245,0 ,0
+ 0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x57, // 57
+};
diff --git a/AT91SAM7S256/Source/Test1.txt b/AT91SAM7S256/Source/Test1.txt
new file mode 100644
index 0000000..018d27d
--- /dev/null
+++ b/AT91SAM7S256/Source/Test1.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, Test1) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x04,0x00, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x80, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+ 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Test2.txt b/AT91SAM7S256/Source/Test2.txt
new file mode 100644
index 0000000..2553335
--- /dev/null
+++ b/AT91SAM7S256/Source/Test2.txt
@@ -0,0 +1,19 @@
+DEFINE_DATA(BMPMAP, Test2) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x04,0x00, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x00, // Graphics Start Y
+ 0x80, // Graphics Width
+ 0x40, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Ui.txt b/AT91SAM7S256/Source/Ui.txt
new file mode 100644
index 0000000..2cf47aa
--- /dev/null
+++ b/AT91SAM7S256/Source/Ui.txt
@@ -0,0 +1,72 @@
+DEFINE_DATA(TXT, Ui) =
+{
+ 0x05,0x00, // Text Format
+ 0x04,0x0D, // Text DataSize
+ 0x01, // ItemsX
+ 0x3D, // ItemsY
+ 0x11, // ItemCharsX
+ 0x01, // ItemCharsY
+BEGIN_DATA
+ 'C','o','n','n','e','c','t','i','n','g', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'L','i','n','e',' ','i','s',' ','b','u','s','y', 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'C','o','n','n','e','c','t','i','o','n','?', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','e','n','d','i','n','g',' ','f','i','l','e', 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'T','u','r','n','i','n','g',' ','o','n', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'T','u','r','n','i','n','g',' ','o','f','f', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','e','a','r','c','h','i','n','g', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'A','b','o','r','t','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','a','i','l','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'M','e','m','o','r','y',' ','f','u','l','l','!', 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','s','a','v','e','d', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','e','x','i','s','t','s', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'o','v','e','r','w','r','i','t','e','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','a','v','e','d',' ','a','s', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','e','x','i','s','t', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'o','v','e','r','w','r','i','t','e','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','d','e','l','e','t','e','d', 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e','s', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'd','e','l','e','t','e','d', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'R','u','n','n','i','n','g', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'A','b','o','r','t','e','d','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'D','o','n','e', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','e','r','r','o','r','!', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'D','e','l','e','t','i','n','g',' ','a','l','l', 0 , 0 , 0 , 0 , 0 ,
+ '%','s',' ','f','i','l','e','s','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'P','r','e','s','s',' ','C','l','e','a','r',' ','t','o', 0 , 0 , 0 ,
+ 's','t','o','p',' ','D','a','t','a','L','o','g','g','i','n','g', 0 ,
+ 'P','o','r','t',' ','o','c','c','u','p','i','e','d','!', 0 , 0 , 0 ,
+ 'H',':','M','M',':','S','S',':','0','0', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'H','H',':','M','M',':','S','S', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','o','u','n','d', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','o','f','t','w','a','r','e', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'N','X','T', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'T','r','y',' ','M','e', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'D','a','t','a','l','o','g', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'P','a','s','s','k','e','y',':', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'F','i','l','e',' ','n','a','m','e',':', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'P','l','e','a','s','e',' ','u','s','e',' ','p','o','r','t',':', 0 ,
+ '1',' ','-',' ','T','o','u','c','h',' ','S','e','n','s','o','r', 0 ,
+ '2',' ','-',' ','S','o','u','n','d',' ','S','e','n','s','o','r', 0 ,
+ '3',' ','-',' ','L','i','g','h','t',' ','S','e','n','s','o','r', 0 ,
+ '4',' ','-',' ','U','l','t','r','a','s','o','n','i','c',' ',' ', 0 ,
+ 'B','/','C',' ','-',' ','L','/','R',' ','m','o','t','o','r','s', 0 ,
+ 'S','e','l','e','c','t', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','e','l','e','c','t', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'S','e','l','e','c','t', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'B','T',' ','s','a','v','e',' ','d','a','t','a', 0 , 0 , 0 , 0 , 0 ,
+ 'e','r','r','o','r','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'B','T',' ','s','t','o','r','e',' ','i','s', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'f','u','l','l',' ','e','r','r','o','r','!', 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'B','T',' ','u','n','k','n','o','w','n', 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'a','d','d','r','.',' ','e','r','r','o','r','!', 0 , 0 , 0 , 0 , 0 ,
+ 'M','e','m','o','r','y',' ','i','s', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'f','u','l','l','!', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
+ 'N','e','v','e','r', 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/Wait.txt b/AT91SAM7S256/Source/Wait.txt
new file mode 100644
index 0000000..abdbd43
--- /dev/null
+++ b/AT91SAM7S256/Source/Wait.txt
@@ -0,0 +1,14 @@
+DEFINE_DATA(BMPMAP, Wait) =
+{
+ 0x02,0x00, // Graphics Format
+ 0x00,0x48, // Graphics DataSize
+ 0x00, // Graphics Start X
+ 0x08, // Graphics Start Y
+ 0x18, // Graphics Width
+ 0x18, // Graphics Height
+BEGIN_DATA
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC3,0x24,0x98,0xC2,0x98,0x24,0xC3,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+END_DATA
+};
diff --git a/AT91SAM7S256/Source/c_button.c b/AT91SAM7S256/Source/c_button.c
new file mode 100644
index 0000000..3145d8f
--- /dev/null
+++ b/AT91SAM7S256/Source/c_button.c
@@ -0,0 +1,134 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date:: 14-11-07 12:40 $
+//
+// Filename $Workfile:: c_button.c $
+//
+// Version $Revision:: 1 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_butt $
+//
+// Platform C
+//
+
+#include "stdconst.h"
+#include "modules.h"
+#include "c_button.h"
+#include "c_button.iom"
+#include "c_button.h"
+#include "d_button.h"
+
+#define BTN_PRESCALER 2
+
+enum
+{
+ LONG_TIME = (2000/BTN_PRESCALER)
+};
+
+static IOMAPBUTTON IOMapButton;
+static VARSBUTTON VarsButton;
+static UBYTE BtnCnt;
+
+const HEADER cButton =
+{
+ 0x00040001L,
+ "Button",
+ cButtonInit,
+ cButtonCtrl,
+ cButtonExit,
+ (void *)&IOMapButton,
+ (void *)&VarsButton,
+ (UWORD)sizeof(IOMapButton),
+ (UWORD)sizeof(VarsButton),
+ 0x0000 //Code size - not used so far
+};
+
+
+void cButtonInit(void* pHeader)
+{
+ UBYTE Tmp;
+
+ for (Tmp = 0; Tmp < NO_OF_BTNS; Tmp++)
+ {
+ IOMapButton.State[Tmp] = 0;
+ IOMapButton.BtnCnt[Tmp].PressedCnt = 0;
+ IOMapButton.BtnCnt[Tmp].LongPressCnt = 0;
+ IOMapButton.BtnCnt[Tmp].ShortRelCnt = 0;
+ IOMapButton.BtnCnt[Tmp].LongRelCnt = 0;
+ VarsButton.Cnt[Tmp] = 0;
+ }
+ VarsButton.OldState = 0;
+ BtnCnt = 0;
+ dButtonInit(BTN_PRESCALER);
+}
+
+void cButtonCtrl(void)
+{
+ UBYTE ButtonState, Tmp, ButtonNo;
+
+ for (Tmp = 0; Tmp < NO_OF_BTNS; Tmp++)
+ {
+ IOMapButton.State[Tmp] &= ~PRESSED_EV;
+ }
+ if (++BtnCnt >= BTN_PRESCALER)
+ {
+ BtnCnt = 0;
+ dButtonRead(&ButtonState);
+
+ ButtonNo = 0x01;
+ for (Tmp = 0; Tmp < NO_OF_BTNS; Tmp++)
+ {
+ if (ButtonState & ButtonNo)
+ {
+ if (LONG_TIME >= (VarsButton.Cnt[Tmp]))
+ {
+ (VarsButton.Cnt[Tmp])++;
+ }
+ IOMapButton.State[Tmp] = PRESSED_STATE;
+ if (!((VarsButton.OldState) & ButtonNo))
+ {
+
+ /* Button just pressed */
+ (IOMapButton.State[Tmp]) |= PRESSED_EV;
+ (IOMapButton.BtnCnt[Tmp].PressedCnt)++;
+ VarsButton.Cnt[Tmp] = 0;
+ }
+ else
+ {
+ if (LONG_TIME == VarsButton.Cnt[Tmp])
+ {
+ IOMapButton.State[Tmp] |= LONG_PRESSED_EV;
+ (IOMapButton.BtnCnt[Tmp].LongPressCnt)++;
+ }
+ }
+ }
+ else
+ {
+ IOMapButton.State[Tmp] = 0x00;
+ if ((VarsButton.OldState) & ButtonNo)
+ {
+ if (VarsButton.Cnt[Tmp] > LONG_TIME)
+ {
+ IOMapButton.State[Tmp] = LONG_RELEASED_EV;
+ (IOMapButton.BtnCnt[Tmp].LongRelCnt)++;
+
+ }
+ else
+ {
+ IOMapButton.State[Tmp] = SHORT_RELEASED_EV;
+ (IOMapButton.BtnCnt[Tmp].ShortRelCnt)++;
+ }
+ }
+ }
+ ButtonNo <<= 1;
+ IOMapButton.BtnCnt[Tmp].RelCnt = ((IOMapButton.BtnCnt[Tmp].ShortRelCnt) + (IOMapButton.BtnCnt[Tmp].LongRelCnt));
+ }
+ VarsButton.OldState = ButtonState;
+ }
+}
+
+void cButtonExit(void)
+{
+ dButtonExit();
+}
diff --git a/AT91SAM7S256/Source/c_button.h b/AT91SAM7S256/Source/c_button.h
new file mode 100644
index 0000000..c33b24d
--- /dev/null
+++ b/AT91SAM7S256/Source/c_button.h
@@ -0,0 +1,37 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date:: 14-11-07 12:40 $
+//
+// Filename $Workfile:: c_button.h $
+//
+// Version $Revision:: 1 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_butt $
+//
+// Platform C
+//
+
+#ifndef C_BUTTON
+#define C_BUTTON
+
+#ifdef INCLUDE_OS
+extern const HEADER cButton;
+#endif
+
+#include "c_button.iom"
+
+
+typedef struct
+{
+ UWORD Cnt[NO_OF_BTNS];
+ UBYTE OldState;
+}VARSBUTTON;
+
+void cButtonInit(void* pHeader);
+void cButtonCtrl(void);
+void cButtonExit(void);
+
+extern const HEADER cButton;
+
+#endif
diff --git a/AT91SAM7S256/Source/c_button.iom b/AT91SAM7S256/Source/c_button.iom
new file mode 100644
index 0000000..640a7cd
--- /dev/null
+++ b/AT91SAM7S256/Source/c_button.iom
@@ -0,0 +1,61 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date:: 14-11-07 12:40 $
+//
+// Filename $Workfile:: c_button.iom $
+//
+// Version $Revision:: 1 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_butt $
+//
+// Platform C
+//
+
+#ifndef CBUTTON_IOM
+#define CBUTTON_IOM
+
+#define pMapButton ((IOMAPBUTTON*)(pHeaders[ENTRY_BUTTON]->pIOMap))
+
+enum
+{
+ BTN1,
+ BTN2,
+ BTN3,
+ BTN4,
+ NO_OF_BTNS
+};
+
+/* Costants related to State */
+enum
+{
+ PRESSED_EV = 0x01,
+ SHORT_RELEASED_EV = 0x02,
+ LONG_PRESSED_EV = 0x04,
+ LONG_RELEASED_EV = 0x08,
+ PRESSED_STATE = 0x80
+};
+
+typedef struct
+{
+ UBYTE PressedCnt;
+ UBYTE LongPressCnt;
+ UBYTE ShortRelCnt;
+ UBYTE LongRelCnt;
+ UBYTE RelCnt;
+ UBYTE SpareOne;
+ UBYTE SpareTwo;
+ UBYTE SpareThree;
+}BTNCNT;
+
+typedef struct
+{
+ BTNCNT BtnCnt[NO_OF_BTNS];
+ UBYTE State[NO_OF_BTNS];
+}IOMAPBUTTON;
+
+
+#endif
+
+
+
diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c
new file mode 100644
index 0000000..7483846
--- /dev/null
+++ b/AT91SAM7S256/Source/c_cmd.c
@@ -0,0 +1,7992 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date: 24-06-09 8:53 $
+//
+// Filename $Workfile:: c_cmd.c $
+//
+// Version $Revision: 14 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $
+//
+// Platform C
+//
+
+//
+// File Description:
+// This file contains the virtual machine implementation to run bytecode
+// programs compatible with LEGO MINDSTORMS NXT Software 2.0.
+//
+// This module (c_cmd) is also responsible for reading the system timer
+// (d_timer) and returning on 1 ms timer boundaries.
+//
+
+#include "stdconst.h"
+#include "modules.h"
+
+#include "c_cmd.iom"
+#include "c_output.iom"
+#include "c_input.iom"
+#include "c_loader.iom"
+#include "c_ui.iom"
+#include "c_sound.iom"
+#include "c_button.iom"
+#include "c_display.iom"
+#include "c_comm.iom"
+#include "c_lowspeed.iom"
+#include "m_sched.h"
+
+#include "c_cmd.h"
+#include "c_cmd_bytecodes.h"
+#include "d_timer.h"
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h> // for sqrt, abs, and trig stuff
+
+#define VMProfilingCode 0
+
+static IOMAPCMD IOMapCmd;
+static VARSCMD VarsCmd;
+static HEADER **pHeaders;
+static ULONG gInstrsToExecute;
+static SLONG gPCDelta;
+#define NUM_INTERP_FUNCS 16
+#define NUM_SHORT_INTERP_FUNCS 8
+#define VAR_INSTR_SIZE 0xE
+// important to cast since most args are assigned from signed value, and locals may be ULONG
+#define GetDataArg(arg) ((UWORD)(arg))
+#if VMProfilingCode
+static ULONG ExecutedInstrs= 0, CmdCtrlTime= 0, OverheadTime= 0, CmdCtrlCalls= 0, LeaveTime= 0, NotFirstCall= 0, LastAvgCount= 0;
+static ULONG CmdCtrlClumpTime[256];
+typedef struct {
+ ULONG Time;
+ ULONG Count;
+ ULONG Avg;
+ ULONG Max;
+} VMInstrProfileInfo;
+static VMInstrProfileInfo InstrProfile[OPCODE_COUNT];
+static VMInstrProfileInfo SysCallProfile[SYSCALL_COUNT];
+static VMInstrProfileInfo InterpFuncProfile[NUM_INTERP_FUNCS];
+static VMInstrProfileInfo ShortInstrProfile[NUM_SHORT_OPCODE_COUNT];
+#endif
+
+#define cCmdDSType(Arg) (VarsCmd.pDataspaceTOC[(Arg)].TypeCode)
+#define cCmdDSScalarPtr(DSElementID, Offset) (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset)
+#define cCmdSizeOf(TC) (TC_Size_Table[(TC)])
+
+#define scalarBinopDispatchMask 0x1
+#define scalarUnop2DispatchMask 0x2
+
+const HEADER cCmd =
+{
+ 0x00010001L,
+ "Command",
+ cCmdInit,
+ cCmdCtrl,
+ cCmdExit,
+ (void *)&IOMapCmd,
+ (void *)&VarsCmd,
+ (UWORD)sizeof(IOMapCmd),
+ (UWORD)sizeof(VarsCmd),
+ 0x0000 //Code size - not used so far
+};
+
+#if ENABLE_VM
+
+// c_cmd_drawing.inc is just another C source file
+// (the graphics implementation was split off for practical file management reasons)
+#include "c_cmd_drawing.inc"
+
+//
+//Function pointers to sub-interpreters
+//This table is indexed by instr size
+//Unary operations can have arity of 1 or 2 (some need a destination)
+//All instructions taking 4 or more operands are handled as "Other"
+// Table uses NoArg for illegal instr sizes such as zero and odd sizes
+//
+static pInterp InterpFuncs[NUM_INTERP_FUNCS] =
+{
+ cCmdInterpNoArg,
+ cCmdInterpNoArg,
+ cCmdInterpNoArg, // size 2
+ cCmdInterpNoArg,
+ cCmdInterpUnop1, // size 4
+ cCmdInterpNoArg,
+ cCmdInterpUnop2, // size 6 general poly is cCmdInterpUnop2, scalar is cCmdInterpScalarUnop2
+ cCmdInterpNoArg,
+ cCmdInterpBinop, // size 8, general poly is cCmdInterpBinop, scalar is cCmdInterpScalarBinop
+ cCmdInterpNoArg,
+ cCmdInterpOther, // size 10
+ cCmdInterpNoArg,
+ cCmdInterpOther, // size 12
+ cCmdInterpNoArg,
+ cCmdInterpOther, // size 14
+ cCmdInterpNoArg
+};
+
+static pInterpShort ShortInterpFuncs[NUM_SHORT_INTERP_FUNCS] =
+{
+ cCmdInterpShortMove,
+ cCmdInterpShortAcquire,
+ cCmdInterpShortRelease,
+ cCmdInterpShortSubCall,
+ cCmdInterpShortError,
+ cCmdInterpShortError,
+ cCmdInterpShortError,
+ cCmdInterpShortError
+};
+
+ULONG TC_Size_Table[]= {
+ 0, // void
+ SIZE_UBYTE,
+ SIZE_SBYTE,
+ SIZE_UWORD,
+ SIZE_SWORD,
+ SIZE_ULONG,
+ SIZE_SLONG,
+ SIZE_UWORD, // array
+ 0, // cluster
+ SIZE_MUTEX,
+ SIZE_FLOAT
+};
+
+
+//
+//Function pointers to SysCall implementations
+//See interpreter for OP_SYSCALL
+//
+static pSysCall SysCallFuncs[SYSCALL_COUNT] =
+{
+ cCmdWrapFileOpenRead,
+ cCmdWrapFileOpenWrite,
+ cCmdWrapFileOpenAppend,
+ cCmdWrapFileRead,
+ cCmdWrapFileWrite,
+ cCmdWrapFileClose, // 5
+ cCmdWrapFileResolveHandle,
+ cCmdWrapFileRename,
+ cCmdWrapFileDelete,
+ cCmdWrapSoundPlayFile,
+ cCmdWrapSoundPlayTone, // 10
+ cCmdWrapSoundGetState,
+ cCmdWrapSoundSetState,
+ cCmdWrapDrawText,
+ cCmdWrapDrawPoint,
+ cCmdWrapDrawLine, // 15
+ cCmdWrapDrawCircle,
+ cCmdWrapDrawRect,
+ cCmdWrapDrawPicture,
+ cCmdWrapSetScreenMode,
+ cCmdWrapReadButton, // 20
+ cCmdWrapCommLSWrite,
+ cCmdWrapCommLSRead,
+ cCmdWrapCommLSCheckStatus,
+ cCmdWrapRandomNumber,
+ cCmdWrapGetStartTick, // 25
+ cCmdWrapMessageWrite,
+ cCmdWrapMessageRead,
+ cCmdWrapCommBTCheckStatus,
+ cCmdWrapCommBTWrite,
+ cCmdWrapCommBTRead, // 30
+ cCmdWrapKeepAlive,
+ cCmdWrapIOMapRead,
+ cCmdWrapIOMapWrite,
+ cCmdWrapColorSensorRead,
+ cCmdWrapCommBTOnOff, // 35
+ cCmdWrapCommBTConnection,
+ cCmdWrapCommHSWrite,
+ cCmdWrapCommHSRead,
+ cCmdWrapCommHSCheckStatus,
+ cCmdWrapReadSemData, //40
+ cCmdWrapWriteSemData,
+ cCmdWrapComputeCalibValue,
+ cCmdWrapUpdateCalibCacheInfo,
+ cCmdWrapDatalogWrite,
+ cCmdWrapDatalogGetTimes, //45
+ cCmdWrapSetSleepTimeout,
+ cCmdWrapListFiles //47
+
+ // don't forget to update SYSCALL_COUNT in c_cmd.h
+};
+
+//
+//Next set of arrays are lookup tables for IOM access bytecodes
+//
+TYPE_CODE IO_TYPES_IN[IO_IN_FIELD_COUNT] =
+{
+ //IO_IN0
+ TC_UBYTE, //IO_IN_TYPE
+ TC_UBYTE, //IO_IN_MODE
+ TC_UWORD, //IO_IN_ADRAW
+ TC_UWORD, //IO_IN_NORMRAW
+ TC_SWORD, //IO_IN_SCALED_VAL
+ TC_UBYTE, //IO_IN_INVALID_DATA
+
+ //IO_IN1
+ TC_UBYTE, //IO_IN_TYPE
+ TC_UBYTE, //IO_IN_MODE
+ TC_UWORD, //IO_IN_ADRAW
+ TC_UWORD, //IO_IN_NORMRAW
+ TC_SWORD, //IO_IN_SCALED_VAL
+ TC_UBYTE, //IO_IN_INVALID_DATA
+
+ //IO_IN2
+ TC_UBYTE, //IO_IN_TYPE
+ TC_UBYTE, //IO_IN_MODE
+ TC_UWORD, //IO_IN_ADRAW
+ TC_UWORD, //IO_IN_NORMRAW
+ TC_SWORD, //IO_IN_SCALED_VAL
+ TC_UBYTE, //IO_IN_INVALID_DATA
+
+ //IO_IN3
+ TC_UBYTE, //IO_IN_TYPE
+ TC_UBYTE, //IO_IN_MODE
+ TC_UWORD, //IO_IN_ADRAW
+ TC_UWORD, //IO_IN_NORMRAW
+ TC_SWORD, //IO_IN_SCALED_VAL
+ TC_UBYTE, //IO_IN_INVALID_DATA
+};
+
+TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
+{
+ //IO_OUT0
+ TC_UBYTE, //IO_OUT_FLAGS
+ TC_UBYTE, //IO_OUT_MODE
+ TC_SBYTE, //IO_OUT_SPEED
+ TC_SBYTE, //IO_OUT_ACTUAL_SPEED
+ TC_SLONG, //IO_OUT_TACH_COUNT
+ TC_ULONG, //IO_OUT_TACH_LIMIT
+ TC_UBYTE, //IO_OUT_RUN_STATE
+ TC_SBYTE, //IO_OUT_TURN_RATIO
+ TC_UBYTE, //IO_OUT_REG_MODE
+ TC_UBYTE, //IO_OUT_OVERLOAD
+ TC_UBYTE, //IO_OUT_REG_P_VAL
+ TC_UBYTE, //IO_OUT_REG_I_VAL
+ TC_UBYTE, //IO_OUT_REG_D_VAL
+ TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
+ TC_SLONG, //IO_OUT_ROTATION_COUNT
+
+ //IO_OUT1
+ TC_UBYTE, //IO_OUT_FLAGS
+ TC_UBYTE, //IO_OUT_MODE
+ TC_SBYTE, //IO_OUT_SPEED
+ TC_SBYTE, //IO_OUT_ACTUAL_SPEED
+ TC_SLONG, //IO_OUT_TACH_COUNT
+ TC_ULONG, //IO_OUT_TACH_LIMIT
+ TC_UBYTE, //IO_OUT_RUN_STATE
+ TC_SBYTE, //IO_OUT_TURN_RATIO
+ TC_UBYTE, //IO_OUT_REG_MODE
+ TC_UBYTE, //IO_OUT_OVERLOAD
+ TC_UBYTE, //IO_OUT_REG_P_VAL
+ TC_UBYTE, //IO_OUT_REG_I_VAL
+ TC_UBYTE, //IO_OUT_REG_D_VAL
+ TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
+ TC_SLONG, //IO_OUT_ROTATION_COUNT
+
+ //IO_OUT2
+ TC_UBYTE, //IO_OUT_FLAGS
+ TC_UBYTE, //IO_OUT_MODE
+ TC_SBYTE, //IO_OUT_SPEED
+ TC_SBYTE, //IO_OUT_ACTUAL_SPEED
+ TC_SLONG, //IO_OUT_TACH_COUNT
+ TC_ULONG, //IO_OUT_TACH_LIMIT
+ TC_UBYTE, //IO_OUT_RUN_STATE
+ TC_SBYTE, //IO_OUT_TURN_RATIO
+ TC_UBYTE, //IO_OUT_REG_MODE
+ TC_UBYTE, //IO_OUT_OVERLOAD
+ TC_UBYTE, //IO_OUT_REG_P_VAL
+ TC_UBYTE, //IO_OUT_REG_I_VAL
+ TC_UBYTE, //IO_OUT_REG_D_VAL
+ TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
+ TC_SLONG, //IO_OUT_ROTATION_COUNT
+};
+
+
+TYPE_CODE * IO_TYPES[2] =
+{
+ IO_TYPES_IN,
+ IO_TYPES_OUT
+};
+
+//Actual pointers filled in during cCmdInit()
+void * IO_PTRS_IN[IO_IN_FIELD_COUNT];
+void * IO_PTRS_OUT[IO_OUT_FIELD_COUNT];
+
+void ** IO_PTRS[2] =
+{
+ IO_PTRS_IN,
+ IO_PTRS_OUT
+};
+
+// Data used to indicate usage of motor ports, or usage requests
+UBYTE gUsageSemData, gRequestSemData;
+
+UBYTE cCmdBTGetDeviceType(UBYTE *pCOD)
+{
+ ULONG COD;
+ UBYTE Result;
+ UBYTE Tmp;
+
+ COD = 0;
+ for (Tmp = 0;Tmp < SIZE_OF_CLASS_OF_DEVICE;Tmp++)
+ {
+ COD <<= 8;
+ COD |= (ULONG)*pCOD;
+ pCOD++;
+ }
+
+ Result = DEVICETYPE_UNKNOWN;
+ if ((COD & 0x00001FFF) == 0x00000804)
+ {
+ Result = DEVICETYPE_NXT;
+ }
+ if ((COD & 0x00001F00) == 0x00000200)
+ {
+ Result = DEVICETYPE_PHONE;
+ }
+ if ((COD & 0x00001F00) == 0x00000100)
+ {
+ Result = DEVICETYPE_PC;
+ }
+
+ return (Result);
+}
+
+//cCmdHandleRemoteCommands is the registered handler for "direct" command protocol packets
+//It is only intended to be called via c_comm's main protocol handler
+UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
+{
+ NXT_STATUS RCStatus = NO_ERR;
+ //Response packet length. Always includes RCStatus byte.
+ ULONG ResponseLen = 1;
+ //Boolean flag to send a response. TRUE unless overridden below.
+ ULONG SendResponse = TRUE;
+ //Boolean flag if we are handling a reply telegram. FALSE unless overridden.
+ ULONG IncomingReply = FALSE;
+ ULONG i, FirstPort, LastPort;
+ UWORD LStatus;
+ UWORD Count, QueueID;
+ UBYTE * pData;
+
+ //Illegal call, give up
+ if (pInBuf == NULL || pLen == NULL)
+ {
+ NXT_BREAK;
+ return (0xFFFF);
+ }
+
+ //No output buffer provided, so skip any work related to returning a response
+ if (pOutBuf == NULL)
+ SendResponse = FALSE;
+
+ //If first byte identifies this as a reply telegram, we have different work to do.
+ if (pInBuf[0] == 0x02)
+ {
+ IncomingReply = TRUE;
+ //Reply telegrams never get responses, even if caller provided a buffer.
+ SendResponse = FALSE;
+ }
+
+ //Advance pInBuf past command type byte
+ pInBuf++;
+
+ if (!IncomingReply)
+ {
+ switch(pInBuf[0])
+ {
+ case RC_START_PROGRAM:
+ {
+ //Check that file exists. If not, return error
+ //!!! Should return standard loader file error in cases like this??
+ //!!! Proper solution would also check file mode to avoid confusing errors
+ if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[1]), NULL, NULL)) != SUCCESS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //Close file handle returned by FINDFIRST
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);
+
+ //File must exist, so inform UI to attempt execution in the usual way (enables consistent feedback)
+ pMapUi->Flags |= UI_EXECUTE_LMS_FILE;
+ strncpy((PSZ)(pMapUi->LMSfilename), (PSZ)(&pInBuf[1]), FILENAME_LENGTH + 1);
+ }
+ break;
+
+ case RC_STOP_PROGRAM:
+ {
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ {
+ RCStatus = ERR_NO_PROG;
+ break;
+ }
+
+ IOMapCmd.DeactivateFlag = TRUE;
+ }
+ break;
+
+ case RC_PLAY_SOUND_FILE:
+ {
+ if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[2]), NULL, NULL)) != SUCCESS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //Close file handle returned by FINDFIRST
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);
+
+ if (pInBuf[1] == FALSE)
+ pMapSound->Mode = SOUND_ONCE;
+ else //Any non-zero value treated as TRUE
+ pMapSound->Mode = SOUND_LOOP;
+
+ strncpy((PSZ)pMapSound->SoundFilename, (PSZ)(&pInBuf[2]), FILENAME_LENGTH + 1);
+ pMapSound->Flags |= SOUND_UPDATE;
+ }
+ break;
+
+ case RC_PLAY_TONE:
+ {
+ pMapSound->Mode = SOUND_TONE;
+ //!!! Range check valid values?
+ memcpy((PSZ)(&(pMapSound->Freq)), (PSZ)(&pInBuf[1]), 2);
+ memcpy((PSZ)(&(pMapSound->Duration)), (PSZ)(&pInBuf[3]), 2);
+
+ pMapSound->Flags |= SOUND_UPDATE;
+ }
+ break;
+
+ case RC_SET_OUT_STATE:
+ {
+ //Don't do anything if illegal port specification is made
+ if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //0xFF is protocol defined to mean "all ports".
+ if (pInBuf[1] == 0xFF)
+ {
+ FirstPort = 0;
+ LastPort = NO_OF_OUTPUTS - 1;
+ }
+ else
+ FirstPort = LastPort = pInBuf[1];
+
+ for (i = FirstPort; i <= LastPort; i++)
+ {
+ pMapOutPut->Outputs[i].Speed = pInBuf[2];
+ pMapOutPut->Outputs[i].Mode = pInBuf[3];
+ pMapOutPut->Outputs[i].RegMode = pInBuf[4];
+ pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5];
+ pMapOutPut->Outputs[i].RunState = pInBuf[6];
+ memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4);
+
+ pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
+ }
+ }
+ break;
+
+ case RC_SET_IN_MODE:
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ //!!! Should check against legal Types and Modes? (bitmask for Modes?)
+ if (i >= NO_OF_INPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ pMapInput->Inputs[i].SensorType = pInBuf[2];
+ pMapInput->Inputs[i].SensorMode = pInBuf[3];
+
+ //Set InvalidData flag automatically since type may have changed
+ pMapInput->Inputs[i].InvalidData = TRUE;
+ }
+ break;
+
+ case RC_GET_OUT_STATE:
+ {
+ if (SendResponse == TRUE)
+ {
+ i = pInBuf[1];
+
+ //Return error and all zeros if illegal port specification is made
+ if (i >= NO_OF_OUTPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ memset(&(pOutBuf[ResponseLen]), 0, 22);
+ ResponseLen += 22;
+ break;
+ }
+
+ //Echo port
+ pOutBuf[ResponseLen] = i;
+ ResponseLen++;
+
+ //Power
+ pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed;
+ ResponseLen++;
+
+ //Mode
+ pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode;
+ ResponseLen++;
+
+ //RegMode
+ pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode;
+ ResponseLen++;
+
+ //TurnRatio
+ pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter;
+ ResponseLen++;
+
+ //RunState
+ pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState;
+ ResponseLen++;
+
+ //TachoLimit ULONG
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4);
+ ResponseLen += 4;
+
+ //TachoCount SLONG
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4);
+ ResponseLen += 4;
+
+ //BlockTachoCount SLONG
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4);
+ ResponseLen += 4;
+
+ //RotationCount SLONG
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4);
+ ResponseLen += 4;
+
+ NXT_ASSERT(ResponseLen == 23);
+ }
+ }
+ break;
+
+ case RC_GET_IN_VALS:
+ {
+ if (SendResponse == TRUE)
+ {
+ i = pInBuf[1];
+
+ //Return error and all zeros if illegal port specification is made
+ if (i >= NO_OF_INPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ memset(&(pOutBuf[ResponseLen]), 0, 13);
+ ResponseLen += 13;
+ break;
+ }
+
+ //Echo port
+ pOutBuf[ResponseLen] = i;
+ ResponseLen++;
+
+ //Set "Valid?" boolean
+ if (pMapInput->Inputs[i].InvalidData)
+ pOutBuf[ResponseLen] = FALSE;
+ else
+ pOutBuf[ResponseLen] = TRUE;
+
+ ResponseLen++;
+
+ //Set "Calibrated?" boolean
+ //!!! "Calibrated?" is a placeholder in the protocol. Always FALSE for now.
+ pOutBuf[ResponseLen] = FALSE;
+ ResponseLen++;
+
+ pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType;
+ ResponseLen++;
+
+ pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode;
+ ResponseLen++;
+
+ //Set Raw, Normalized, and Scaled values
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2);
+ ResponseLen += 2;
+
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2);
+ ResponseLen += 2;
+
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorValue)), 2);
+ ResponseLen += 2;
+
+ //!!! Return normalized raw value in place of calibrated value for now -- see comment above
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2);
+ ResponseLen += 2;
+
+ NXT_ASSERT(ResponseLen == 14);
+ }
+ }
+ break;
+
+ case RC_RESET_IN_VAL:
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_INPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //Clear SensorValue to zero. Leave Raw and Normalized as-is, since they never accumulate running values.
+ pMapInput->Inputs[i].SensorValue = 0;
+ }
+ break;
+
+ case RC_MESSAGE_WRITE:
+ {
+ QueueID = pInBuf[1];
+ Count = pInBuf[2];
+ pData = &(pInBuf[3]);
+
+ //If Count is illegal or MsgData is not null-terminated,
+ // we can't accept it as a valid string
+ if (Count == 0 || Count > MAX_MESSAGE_SIZE || pData[Count - 1] != 0x00)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ RCStatus = cCmdMessageWrite(QueueID, pData, Count);
+
+ //ERR_MEM here means we must compact the dataspace and retry message write
+ if (RCStatus == ERR_MEM)
+ {
+ cCmdDSCompact();
+ RCStatus = cCmdMessageWrite(QueueID, pData, Count);
+ }
+ }
+ break;
+
+ case RC_RESET_POSITION:
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_OUTPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //pInBuf[2] is a selector
+ //FALSE: Position relative to start of last program
+ //TRUE: Position relative to start of last motor control block
+ if (pInBuf[2] == FALSE)
+ {
+ pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_ROTATION_COUNT;
+ }
+ else
+ {
+ pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_BLOCK_COUNT;
+ }
+ }
+ break;
+
+ case RC_GET_BATT_LVL:
+ {
+ if (SendResponse == TRUE)
+ {
+ //Return BatteryVoltage directly from IOMapUI, in mV
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pMapUi->BatteryVoltage), 2);
+ ResponseLen += 2;
+ }
+ }
+ break;
+
+ case RC_STOP_SOUND:
+ {
+ //Tell sound module to stop playback, no questions asked
+ pMapSound->State = SOUND_STOP;
+ }
+ break;
+
+ case RC_KEEP_ALIVE:
+ {
+ pMapUi->Flags |= UI_RESET_SLEEP_TIMER;
+
+ if (SendResponse == TRUE)
+ {
+ //Convert to milliseconds to match external conventions
+ i = (pMapUi->SleepTimeout * 60 * 1000);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&i, 4);
+ ResponseLen += 4;
+ }
+ }
+ break;
+
+ case RC_LS_GET_STATUS:
+ {
+ if (SendResponse == TRUE)
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ RCStatus = cCmdLSCheckStatus(i);
+
+ pOutBuf[ResponseLen] = cCmdLSCalcBytesReady(i);
+ ResponseLen++;
+ }
+ }
+ break;
+
+ case RC_LS_WRITE:
+ {
+ i = pInBuf[1];
+ Count = pInBuf[2];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3]);
+ }
+ break;
+
+ case RC_LS_READ:
+ {
+ if (SendResponse == TRUE)
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ //Get channel status and number of bytes available to read
+ RCStatus = cCmdLSCheckStatus(i);
+ Count = cCmdLSCalcBytesReady(i);
+
+ pOutBuf[ResponseLen] = (UBYTE)Count;
+ ResponseLen++;
+
+ //If channel is ready and has data ready for us, put the data into outgoing buffer
+ if (!IS_ERR(RCStatus) && Count > 0)
+ {
+ RCStatus = cCmdLSRead(i, (UBYTE)Count, &(pOutBuf[ResponseLen]));
+ ResponseLen += Count;
+ }
+
+ //Pad remaining data bytes with zeroes
+ Count = 16 - Count;
+ memset(&(pOutBuf[ResponseLen]), 0, Count);
+ ResponseLen += Count;
+ }
+ }
+ break;
+
+ case RC_GET_CURR_PROGRAM:
+ {
+ if (SendResponse == TRUE)
+ {
+ //If there's no active program, return error and empty name buffer
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ {
+ RCStatus = ERR_NO_PROG;
+ memset(&(pOutBuf[ResponseLen]), 0, FILENAME_LENGTH + 1);
+ }
+ //Else, copy out stashed program name
+ else
+ {
+ strncpy((PSZ)(&(pOutBuf[ResponseLen])), (PSZ)(VarsCmd.ActiveProgName), FILENAME_LENGTH + 1);
+ }
+
+ //Regardless, we've copied out a filename's worth of bytes...
+ ResponseLen += FILENAME_LENGTH + 1;
+ }
+ }
+ break;
+
+ case RC_MESSAGE_READ:
+ {
+ if (SendResponse == TRUE)
+ {
+ QueueID = pInBuf[1];
+
+ //Fill in response with remote mailbox number so remote device knows where to store this message.
+ pOutBuf[ResponseLen] = pInBuf[2];
+ ResponseLen++;
+
+ RCStatus = cCmdMessageGetSize(QueueID, &Count);
+ pOutBuf[ResponseLen] = Count;
+ ResponseLen++;
+
+ if (!IS_ERR(RCStatus) && Count > 0)
+ {
+ pData = &(pOutBuf[ResponseLen]);
+ RCStatus = cCmdMessageRead(QueueID, pData, Count, (pInBuf[3]));
+ //If cCmdMessageRead encountered an error, there is no real data in the buffer, so clear it out (below)
+ if (IS_ERR(RCStatus))
+ Count = 0;
+ else
+ ResponseLen += Count;
+ }
+
+ //Pad remaining data bytes with zeroes
+ Count = MAX_MESSAGE_SIZE - Count;
+ memset(&(pOutBuf[ResponseLen]), 0, Count);
+ ResponseLen += Count;
+ }
+ }
+ break;
+
+ // remote-only command to read from datalog buffer
+ // pInBuf[1] = Remove? (bool)
+ case RC_DATALOG_READ:
+ {
+ if (SendResponse == TRUE)
+ {
+ RCStatus = cCmdDatalogGetSize(&Count);
+ pOutBuf[ResponseLen] = Count;
+ ResponseLen++;
+
+ if (!IS_ERR(RCStatus) && Count > 0)
+ {
+ pData = &(pOutBuf[ResponseLen]);
+ RCStatus = cCmdDatalogRead(pData, Count, (pInBuf[1]));
+ //If cCmdDatalogRead encountered an error, there is no real data in the buffer, so clear it out (below)
+ if (IS_ERR(RCStatus))
+ Count = 0;
+ else
+ ResponseLen += Count;
+ }
+
+ //Pad remaining data bytes with zeroes
+ Count = MAX_DATALOG_SIZE - Count;
+ memset(&(pOutBuf[ResponseLen]), 0, Count);
+ ResponseLen += Count;
+ }
+ }
+ break;
+ case RC_DATALOG_SET_TIMES:
+ {
+ //SyncTime SLONG
+ memcpy((PSZ)&IOMapCmd.SyncTime, (PSZ)&(pInBuf[1]), 4);
+ IOMapCmd.SyncTick= dTimerReadNoPoll();
+ }
+ break;
+
+ case RC_BT_GET_CONN_COUNT:
+ if (SendResponse == TRUE) {
+ pOutBuf[ResponseLen]= SIZE_OF_BT_CONNECT_TABLE;
+ ResponseLen++;
+ }
+ break;
+ case RC_BT_GET_CONN_NAME: // param in is index, param out is name
+ if (SendResponse == TRUE) { // get index from inbuf
+ i = pInBuf[1];
+ if(i < SIZE_OF_BT_CONNECT_TABLE) { // unsigned, so guaranteed >= 0
+ pOutBuf[ResponseLen] = cCmdBTGetDeviceType(pMapComm->BtConnectTable[i].ClassOfDevice);
+ memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtConnectTable[i].Name), SIZE_OF_BT_NAME + 1);
+ ResponseLen += SIZE_OF_BT_NAME + 2;
+ }
+ else {
+ pOutBuf[ResponseLen] = 0;
+ ResponseLen += SIZE_OF_BT_NAME + 2;
+ }
+ }
+ break;
+ case RC_BT_GET_CONTACT_COUNT:
+ if (SendResponse == TRUE) {
+ pOutBuf[ResponseLen]= SIZE_OF_BT_DEVICE_TABLE;
+ ResponseLen++;
+ }
+ break;
+ case RC_BT_GET_CONTACT_NAME:
+ if (SendResponse == TRUE) { // get index from inbuf
+ i = pInBuf[1];
+ if(i < SIZE_OF_BT_DEVICE_TABLE && (pMapComm->BtDeviceTable[i].DeviceStatus & BT_DEVICE_KNOWN)) { // unsigned, so guaranteed >= 0
+ (pOutBuf[ResponseLen])= cCmdBTGetDeviceType(pMapComm->BtDeviceTable[i].ClassOfDevice);
+ memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtDeviceTable[i].Name), SIZE_OF_BT_NAME + 1);
+ ResponseLen += SIZE_OF_BT_NAME + 2;
+ }
+ else
+ {
+ pOutBuf[ResponseLen] = 0;
+ memset((PSZ)(&(pOutBuf[ResponseLen+1])), 0, SIZE_OF_BT_NAME + 1);
+ ResponseLen += SIZE_OF_BT_NAME + 2;
+ }
+ }
+ break;
+ case RC_SET_PROPERTY: // label/value pairs
+ i = pInBuf[1];
+ switch(i) {
+ case RC_PROP_BTONOFF: {
+ UWORD retVal, status;
+ if(pInBuf[2])
+ status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
+ else
+ status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);
+
+ RCStatus= (status == SUCCESS) ? retVal : status;
+ }
+ break;
+ case RC_PROP_SOUND_LEVEL: {
+ UBYTE volume= pInBuf[2];
+ if(volume > 4)
+ volume= 4;
+ pMapSound->Volume= volume; // apparently stored in two places
+ pMapUi->Volume= volume;
+ }
+ break;
+ case RC_PROP_SLEEP_TIMEOUT: { // ulong millisecs to sleep
+ ULONG value;
+ memcpy((PSZ)&value, (PSZ)&(pInBuf[2]), 4);
+ pMapUi->SleepTimeout= value / 60000;
+ }
+ break;
+ default:
+ //Unknown property -- still inform client to not expect any response bytes
+ NXT_BREAK;
+ RCStatus = ERR_RC_UNKNOWN_CMD;
+ break;
+ }
+ break;
+ case RC_GET_PROPERTY: // label/value pairs
+ if (SendResponse == TRUE) { // get index from inbuf
+ i = pInBuf[1];
+ switch(i) {
+ case RC_PROP_BTONOFF:
+ pOutBuf[ResponseLen]= pMapUi->BluetoothState != BT_STATE_OFF;
+ ResponseLen++;
+ break;
+ case RC_PROP_SOUND_LEVEL: {
+ pOutBuf[ResponseLen]= pMapSound->Volume;
+ ResponseLen++;
+ }
+ break;
+ case RC_PROP_SLEEP_TIMEOUT: {
+ ULONG value= (pMapUi->SleepTimeout * 60 * 1000);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&value, 4);
+ ResponseLen += 4;
+ }
+ break;
+ default:
+ //Unknown property -- still inform client to not expect any response bytes
+ NXT_BREAK;
+ RCStatus = ERR_RC_UNKNOWN_CMD;
+ break;
+ }
+ }
+ break;
+ case RC_UPDATE_RESET_COUNT:
+ {
+ i = pInBuf[1];
+
+ //Don't do anything if illegal port specification is made
+ if (i >= NO_OF_OUTPUTS)
+ {
+ RCStatus = ERR_RC_ILLEGAL_VAL;
+ break;
+ }
+
+ pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_COUNT;
+ }
+ break;
+ default:
+ {
+ //Unknown remote command -- still inform client to not expect any response bytes
+ NXT_BREAK;
+ RCStatus = ERR_RC_UNKNOWN_CMD;
+ }
+ break;
+ };
+ }
+ //Handle reply telegrams
+ else
+ {
+ switch(pInBuf[0])
+ {
+ case RC_MESSAGE_READ:
+ {
+ QueueID = pInBuf[2];
+ Count = pInBuf[3];
+ pData = &(pInBuf[4]);
+
+ //This is a response to our request to read a message from a remote mailbox.
+ //If telegram looks valid, write the resulting message into our local mailbox.
+ //(If MsgData is not null-terminated, we can't accept it as a valid string.)
+ if (!IS_ERR((SBYTE)(pInBuf[1]))
+ && Count > 0
+ && Count <= MAX_MESSAGE_SIZE
+ && pData[Count - 1] == 0x00)
+ {
+ RCStatus = cCmdMessageWrite(QueueID, pData, Count);
+
+ //ERR_MEM here means we must compact the dataspace
+ if (RCStatus == ERR_MEM)
+ {
+ cCmdDSCompact();
+ RCStatus = cCmdMessageWrite(QueueID, pData, Count);
+ }
+ }
+
+ //If telegram doesn't check out, do nothing. No errors are ever returned for reply telegrams.
+ }
+ break;
+
+ default:
+ {
+ //Unhandled reply telegram. Do nothing.
+ //!!! Could/should stash unhandled/all replies somewhere so a syscall could read them
+ }
+ break;
+ };
+ }
+
+ if (SendResponse == TRUE)
+ {
+ //Return response length (pointer checked above)
+ *pLen = (UBYTE)ResponseLen;
+ //Fill in status byte
+ pOutBuf[0] = (UBYTE)(RCStatus);
+ }
+ else
+ *pLen = 0;
+
+ return (0);
+}
+
+
+//
+// Standard interface functions
+//
+
+void cCmdInit(void* pHeader)
+{
+ ULONG i;
+
+ pHeaders = pHeader;
+
+ IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands;
+
+#if defined(ARM_DEBUG)
+ //Init run-time assert tracking variables
+ VarsCmd.AssertFlag = FALSE;
+ VarsCmd.AssertLine = 0;
+#endif
+
+ //Initialize IO_PTRS_OUT
+ for (i = 0; i < NO_OF_OUTPUTS; i++)
+ {
+ IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Flags);
+ IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Mode);
+ IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Speed);
+ IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].ActualSpeed);
+ IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoCnt);
+ IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoLimit);
+ IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RunState);
+ IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].SyncTurnParameter);
+ IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegMode);
+ IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Overloaded);
+ IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegPParameter);
+ IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegIParameter);
+ IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegDParameter);
+ IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].BlockTachoCount);
+ IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RotationCount);
+ }
+
+ //Initialize IO_PTRS_IN
+ for (i = 0; i < NO_OF_INPUTS; i++)
+ {
+ IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType);
+ IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode);
+ IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw);
+ IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw);
+ IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue);
+ IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData);
+ }
+
+ //Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd)
+ cCmdInitPool();
+ cCmdDeactivateProgram();
+
+ //Global state variables for BlueTooth communication.
+ VarsCmd.CommStat = (SWORD)SUCCESS;
+ VarsCmd.CommStatReset = (SWORD)BTBUSY;
+ VarsCmd.CommCurrConnection = 1;
+
+ //Global flags for various reset and bookkeeping scenarios
+ VarsCmd.DirtyComm = FALSE;
+ VarsCmd.DirtyDisplay = FALSE;
+
+ VarsCmd.VMState = VM_IDLE;
+
+#if defined (ARM_NXT)
+ //Make sure Pool is long-aligned
+ NXT_ASSERT(!((ULONG)(POOL_START) % SIZE_SLONG));
+#endif
+
+ IOMapCmd.ProgStatus = PROG_IDLE;
+ IOMapCmd.ActivateFlag = FALSE;
+ IOMapCmd.Awake = TRUE;
+
+ //Default offsets explicitly chosen to cause an error if used with IOMAPREAD/IOMAPWRITE
+ //Real values will be set when programs run and/or the DS is re-arranged.
+ IOMapCmd.OffsetDVA = 0xFFFF;
+ IOMapCmd.OffsetDS = 0xFFFF;
+
+ //Initialize format string and clear out FileName string
+ strncpy((PSZ)(IOMapCmd.FormatString), VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE);
+ memset(IOMapCmd.FileName, 0, sizeof(IOMapCmd.FileName));
+
+ dTimerInit();
+ IOMapCmd.Tick = dTimerRead();
+ IOMapCmd.SyncTime= 0;
+ IOMapCmd.SyncTick= 0;
+
+ return;
+}
+
+
+void cCmdCtrl(void)
+{
+ NXT_STATUS Status = NO_ERR;
+
+ switch (VarsCmd.VMState)
+ {
+ case VM_RUN_FREE:
+ case VM_RUN_SINGLE:
+ {
+ #if VMProfilingCode
+ ULONG EnterTime= dTimerReadHiRes(), FinishTime;
+ CmdCtrlCalls ++;
+#endif
+ ULONG Continue;
+
+#if VM_BENCHMARK
+ //IOMapCmd.Tick currently holds the tick from the end of last cCmdCtrl call.
+ //If we don't come back here before dTimerRead() increments, the m_sched loop has taken *at least* 1 ms.
+ if (IOMapCmd.Tick != dTimerRead())
+ {
+ VarsCmd.OverTimeCount++;
+ //Record maximum magnitude of schedule loop overage, in millisecs
+ if (dTimerRead() - IOMapCmd.Tick > VarsCmd.MaxOverTimeLength)
+ VarsCmd.MaxOverTimeLength = dTimerRead() - IOMapCmd.Tick;
+ }
+ VarsCmd.CmdCtrlCount++;
+#endif
+ //Abort current program if cancel button is pressed
+ if (IOMapCmd.DeactivateFlag == TRUE || pMapButton->State[BTN1] & PRESSED_EV)
+ {
+ IOMapCmd.DeactivateFlag = FALSE;
+
+ //Clear pressed event so it doesn't get double-counted by UI
+ pMapButton->State[BTN1] &= ~PRESSED_EV;
+
+ //Go to VM_RESET1 state and report abort
+ VarsCmd.VMState = VM_RESET1;
+ IOMapCmd.ProgStatus = PROG_ABORT;
+ break;
+ }
+
+ //Assert that we have an active program
+ NXT_ASSERT(VarsCmd.ActiveProgHandle != NOT_A_HANDLE);
+
+ //Handle any resting clumps that are ready to awaken
+ cCmdCheckRestQ(IOMapCmd.Tick); // not using result, yet
+ //Execute from at least one clump
+ do
+ {
+ //Execute instructions from a clump up to INSTR_MAX, to end of millisec,
+ //Finishing/suspending a clump, BREAKOUT_REQ, or any errors will cause a return
+#if VMProfilingCode
+ ULONG ClumpEnterTime= dTimerReadHiRes();
+ CLUMP_ID clump= VarsCmd.RunQ.Head;
+#endif
+ Status = cCmdInterpFromClump();
+#if VMProfilingCode
+ CmdCtrlClumpTime[clump] += dTimerReadHiRes() - ClumpEnterTime;
+#endif
+
+ //If RunQ and RestQ are empty, program is done, or wacko
+ if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head)) {
+ Continue = FALSE;
+ if(!cCmdIsClumpIDSane(VarsCmd.RestQ.Head)) {
+ VarsCmd.VMState = VM_RESET1;
+ IOMapCmd.ProgStatus = PROG_OK;
+ }
+ }
+ else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE)
+ Continue = TRUE; // queue isn't empty, didn't timeout
+ //Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request
+ else if (Status == ROTATE_QUEUE) { // done and suspend do their own
+ cCmdRotateQ();
+ Continue= TRUE;
+ }
+ else if (Status == TIMES_UP) {
+ cCmdRotateQ();
+ Continue = FALSE;
+ }
+ else if (IS_ERR(Status)) // mem error is handled in InterpFromClump if possible
+ {
+ Continue = FALSE;
+ VarsCmd.VMState = VM_RESET1;
+ IOMapCmd.ProgStatus = PROG_ERROR;
+ }
+ else if (Status == STOP_REQ)
+ {
+ Continue = FALSE;
+ VarsCmd.VMState = VM_RESET1;
+ IOMapCmd.ProgStatus = PROG_OK;
+ }
+ else if (Status == BREAKOUT_REQ)
+ {
+ Continue = FALSE;
+ }
+ } while (Continue == TRUE);
+#if VMProfilingCode
+ FinishTime= dTimerReadHiRes();
+ if(NotFirstCall)
+ OverheadTime += EnterTime - LeaveTime;
+ else
+ NotFirstCall= 1;
+ CmdCtrlTime += FinishTime - EnterTime;
+ LeaveTime= FinishTime;
+#endif
+ // May busy wait to postpone to 1ms schedule
+ while (IOMapCmd.Tick == dTimerRead());
+ }
+ break;
+ case VM_IDLE:
+ {
+ //If there's a new program to activate...
+ if (IOMapCmd.ActivateFlag == TRUE)
+ {
+ //Clear flag so we only activate once per new file
+ IOMapCmd.ActivateFlag = FALSE;
+
+ Status = cCmdActivateProgram(IOMapCmd.FileName);
+
+ //If we hit an activation error:
+ //1. Set PROG_ERROR status
+ //2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI
+ if (IS_ERR(Status))
+ {
+ IOMapCmd.ProgStatus = PROG_ERROR;
+ VarsCmd.VMState = VM_RESET1;
+ }
+ //Else start running program
+ else
+ {
+ VarsCmd.VMState = VM_RUN_FREE;
+ IOMapCmd.ProgStatus = PROG_RUNNING;
+ VarsCmd.StartTick = IOMapCmd.Tick;
+ if(VarsCmd.VMState == VM_RUN_FREE)
+ gInstrsToExecute = 20;
+ else
+ gInstrsToExecute= 1;
+
+#if VM_BENCHMARK
+ //Re-init benchmark
+ VarsCmd.InstrCount = 0;
+ VarsCmd.Average = 0;
+ VarsCmd.OverTimeCount = 0;
+ VarsCmd.MaxOverTimeLength = 0;
+ VarsCmd.CmdCtrlCount = 0;
+ VarsCmd.CompactionCount = 0;
+ VarsCmd.LastCompactionTick = 0;
+ VarsCmd.MaxCompactionTime = 0;
+ memset(VarsCmd.OpcodeBenchmarks, 0, sizeof(VarsCmd.OpcodeBenchmarks));
+ memset(VarsCmd.SyscallBenchmarks, 0, sizeof(VarsCmd.SyscallBenchmarks));
+#endif
+ //Reset devices to a known state before we begin running
+ cCmdResetDevices();
+
+ pMapUi->Flags |= (UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);
+ }
+ }
+ while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
+ }
+ break;
+
+ //Initialize VM internal state data and devices which must respond immediately to program ending
+ case VM_RESET1:
+ {
+ //If we aborted a program, reset devices (specifically, motors) immediately
+ //Otherwise, wait for UI to put us into PROG_RESET (gives motors a chance to brake before setting to coast)
+ //!!! This means cCmdResetDevices will get called twice on abort. Should not be a big deal.
+ if (IOMapCmd.ProgStatus == PROG_ABORT)
+ cCmdResetDevices();
+
+ //Reenable UI access to buttons
+ pMapUi->Flags &= ~(UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);
+
+#if VM_BENCHMARK
+ if (IOMapCmd.Tick != VarsCmd.StartTick)
+ VarsCmd.Average = VarsCmd.InstrCount / (IOMapCmd.Tick - VarsCmd.StartTick);
+ else
+ //It appears that we finished in 0 milliseconds. Very unlikely on ARM, so set a flag value.
+ VarsCmd.Average = 0xFFFFFFFF;
+
+ cCmdWriteBenchmarkFile();
+#endif
+
+ //Re-initialize program state data (contents of memory pool preserved)
+ //!!! Skip this step in simulator builds so helper access methods still work
+#ifndef SIM_NXT
+ cCmdDeactivateProgram();
+#endif //SIM_NXT
+
+ //If this program has taken over the display, reset it for the UI
+ cCmdRestoreDefaultScreen();
+
+ //Stop any currently playing sound and re-init volume according to UI prefs
+ pMapSound->State = SOUND_STOP;
+ pMapSound->Volume = pMapUi->Volume;
+
+ //Artificially set CommStatReset to BTBUSY to force at least one SETCMDMODE call (see VM_RESET2 case)
+ VarsCmd.CommStatReset = (SWORD)BTBUSY;
+
+ VarsCmd.VMState = VM_RESET2;
+ while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
+ }
+ break;
+
+ case VM_RESET2:
+ {
+ //Reset BlueCore into "command mode" (close any open streams)
+ //Since SETCMDMODE subject to BTBUSY, we may need to make multiple calls
+ //Any CommStatReset value other than BTBUSY means our request was accepted
+ //Assumptions:
+ //Process should never take longer than UI timeout (see below), but if it does,
+ // we could be left with the stream open to an NXT peer and block out the PC.
+ //Also assuming that once SETCMDMODE request is accepted, it never fails.
+ if (VarsCmd.CommStatReset == (SWORD)BTBUSY && VarsCmd.DirtyComm == TRUE)
+ pMapComm->pFunc(SETCMDMODE, 0, 0, 0, NULL, (UWORD*)&(VarsCmd.CommStatReset));
+
+ //If UI is done displaying ending program status, move on.
+ if (IOMapCmd.ProgStatus == PROG_RESET)
+ {
+ //Reset devices whenever a program ends for any reason
+ cCmdResetDevices();
+
+ VarsCmd.DirtyComm = FALSE;
+
+ //Go to VM_IDLE state
+ VarsCmd.VMState = VM_IDLE;
+ IOMapCmd.ProgStatus = PROG_IDLE;
+ }
+ while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
+ }
+ break;
+ }//END state machine switch
+
+ //Set tick to new value for next time 'round
+ IOMapCmd.Tick = dTimerReadNoPoll();
+
+ return;
+}
+
+
+void cCmdExit(void)
+{
+ dTimerExit();
+
+ return;
+}
+
+
+NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize,
+ PROG_FILE_OFFSETS* pFileOffsets)
+{
+ ULONG i;
+ UBYTE * pCursor;
+ UWORD CurrOffset = 0;
+ UBYTE DepCount;
+ UWORD DopeVectorOffset;
+ UWORD FileClumpCount;
+ UBYTE FileMajor, FileMinor,
+ CompatibleMinor, CompatibleMajor,
+ CurrentMajor;
+
+ NXT_ASSERT(pData != NULL);
+
+ if (strncmp((PSZ)pData, "NXTBINARY", VM_FORMAT_STRING_SIZE) == 0)
+ {
+ ULONG NativeOffset;
+ pCursor = (pData + 12);
+ NativeOffset = (ULONG)(*pCursor);
+ void (*native)(ULONG, ULONG) = (void (*)())(pData + NativeOffset);
+ (*native)((ULONG)pData, DataSize);
+ NXT_BREAK;
+ return (ERR_VER);
+ }
+ //Assign pCursor to point to version word inside file header
+ pCursor = (pData + VM_FORMAT_STRING_SIZE - 2);
+
+ //Decode version numbers into comparable bytes
+ FileMajor = *pCursor;
+ FileMinor = *(pCursor + 1);
+ CompatibleMajor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION >> 8);
+ CompatibleMinor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION);
+ CurrentMajor = (UBYTE)(FIRMWAREVERSION >> 8);
+ //CurrentMinor = (UBYTE)(FIRMWAREVERSION);
+
+ //Return ERR_VER if file lacks proper format string or version number
+ //!!! Only checking major version recommended for future development
+ if (strncmp((PSZ)pData, VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE)
+ || FileMajor < CompatibleMajor || FileMinor < CompatibleMinor
+ || FileMajor > CurrentMajor)
+ {
+ NXT_BREAK;
+ return (ERR_VER);
+ }
+
+ //Advance CurrOffset past header information
+ CurrOffset += VM_FORMAT_STRING_SIZE;
+
+ //
+ //Initialize bookkeeping variables
+ //
+ VarsCmd.DataspaceCount = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ VarsCmd.DataspaceSize = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ VarsCmd.DSStaticSize = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ pFileOffsets->DSDefaultsSize = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ pFileOffsets->DynamicDefaults = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ pFileOffsets->DynamicDefaultsSize = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ VarsCmd.MemMgr.Head = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ VarsCmd.MemMgr.Tail = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ DopeVectorOffset = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ //!!! Odd code here to deal with type mismatch between file format and CLUMP_ID typedef.
+ //Neither is trivial to change, so it's best to just check the data for consistency here.
+ FileClumpCount = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ //Must have at least one clump and count can't exceed the NOT_A_CLUMP sentinel
+ if (FileClumpCount == 0 || FileClumpCount >= NOT_A_CLUMP)
+ return (ERR_FILE);
+ else
+ VarsCmd.AllClumpsCount = (CLUMP_ID)FileClumpCount;
+
+ VarsCmd.CodespaceCount = *((UWORD*)(pData + CurrOffset));
+ CurrOffset += 2;
+
+ //Can't have a valid program with no code
+ if (VarsCmd.CodespaceCount == 0)
+ return (ERR_FILE);
+
+ //
+ // Now, calculate offsets for each data segment in the file
+ //
+
+ CurrOffset += CurrOffset % 2;
+ pFileOffsets->DSTOC = CurrOffset;
+ CurrOffset += VarsCmd.DataspaceCount * sizeof(DS_TOC_ENTRY);
+
+ CurrOffset += CurrOffset % 2;
+ pFileOffsets->DSDefaults = CurrOffset;
+ CurrOffset += pFileOffsets->DSDefaultsSize;
+
+ //ClumpRecs must be aligned on even boundaries
+ CurrOffset += CurrOffset % 2;
+ pFileOffsets->Clumps = CurrOffset;
+
+ //Set cursor to start of clump records
+ pCursor = pData + CurrOffset;
+
+ //Set CurrOffset to start of dependent lists
+ CurrOffset += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;
+
+ //Read dependent count from each clump record, advancing CurrOffset accordingly
+ for (i = 0; i < VarsCmd.AllClumpsCount; i++)
+ {
+ DepCount = *(pCursor + 1);
+ CurrOffset += DepCount;
+ pCursor += VM_FILE_CLUMP_REC_SIZE;
+ }
+
+ //Codespace must be aligned on even boundary
+ CurrOffset += CurrOffset % 2;
+ pFileOffsets->Codespace = CurrOffset;
+
+ //No need to read through codespace, but make sure CurrOffset ended up sane
+ //If not, something went wrong reading the header information
+ if (CurrOffset != (DataSize - VarsCmd.CodespaceCount * 2))
+ {
+ NXT_BREAK;
+ return (ERR_FILE);
+ }
+
+ //
+ // Finally, update VarsCmd fields
+ //
+
+ VarsCmd.RunQ.Head = NOT_A_CLUMP;
+ VarsCmd.RunQ.Tail = NOT_A_CLUMP;
+ VarsCmd.RestQ.Head = NOT_A_CLUMP;
+ VarsCmd.RestQ.Tail = NOT_A_CLUMP;
+
+ //Reset codespace pointer
+ VarsCmd.pCodespace = (CODE_WORD*)(pData + pFileOffsets->Codespace);
+
+ //...placing clump records first...
+ VarsCmd.pAllClumps = (CLUMP_REC*)(VarsCmd.Pool + VarsCmd.PoolSize);
+ VarsCmd.PoolSize += VarsCmd.AllClumpsCount * sizeof(CLUMP_REC);
+
+ //...then DSTOC...
+ VarsCmd.pDataspaceTOC = (DS_TOC_ENTRY*)(pData + pFileOffsets->DSTOC);
+
+ //...then the dataspace itself
+ ALIGN_TO_MOD(VarsCmd.PoolSize, POOL_ALIGN);
+ VarsCmd.pDataspace = (VarsCmd.Pool + VarsCmd.PoolSize);
+ IOMapCmd.OffsetDS = (UWORD)((ULONG)(VarsCmd.pDataspace) - (ULONG)&(IOMapCmd));
+ VarsCmd.PoolSize += VarsCmd.DataspaceSize;
+
+ //init rest of MemMgr
+ VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + DopeVectorOffset);
+ IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
+ VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;
+
+
+ if (VarsCmd.PoolSize > POOL_MAX_SIZE)
+ {
+ NXT_BREAK;
+ return (ERR_FILE);
+ }
+
+ return (NO_ERR);
+}
+
+
+//!!! Recursive function
+NXT_STATUS cCmdInflateDSDefaults(UBYTE* pDSDefaults, UWORD *pDefaultsOffset, DS_ELEMENT_ID DSElementID)
+{
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode;
+ UWORD i, Count;
+ UBYTE *pVal;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (TypeCode > TC_LAST_VALID)
+ return ERR_INSTR;
+ else if (TypeCode == TC_CLUSTER)
+ {
+ Count = cCmdClusterCount(DSElementID);
+ //Advance DSElementID to sub-type
+ DSElementID = INC_ID(DSElementID);
+ //Loop through sub-types, inflate recursively
+ for (i = 0; i < Count; i++)
+ {
+ Status = cCmdInflateDSDefaults(pDSDefaults, pDefaultsOffset, DSElementID);
+ if (IS_ERR(Status))
+ return Status;
+ DSElementID = cCmdNextDSElement(DSElementID);
+ }
+ }
+ else
+ {
+ if (TypeCode == TC_ARRAY)
+ {
+ //Resolve pointer to DVIndex
+ pVal = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset;
+ }
+ else
+ {
+ pVal = cCmdResolveDataArg(DSElementID, 0, NULL);
+ }
+
+ //Check if the element has the "default default"
+ if (VarsCmd.pDataspaceTOC[DSElementID].Flags & DS_DEFAULT_DEFAULT)
+ {
+ //Fill element with the "default default" of zero
+ memset(pVal, 0, cCmdSizeOf(TypeCode));
+ }
+ else
+ {
+ //Get default from stream
+ memmove(pVal, pDSDefaults + *pDefaultsOffset, cCmdSizeOf(TypeCode));
+ *pDefaultsOffset += cCmdSizeOf(TypeCode);
+ }
+ }
+
+ //!!! Currently will always return NO_ERR
+ return Status;
+}
+
+void cCmdRefreshActiveClump(CLUMP_ID CurrID)
+{
+ CLUMP_REC * clumpRecPtr= &(VarsCmd.pAllClumps[CurrID]);
+
+ if(clumpRecPtr->clumpScalarDispatchHints & scalarBinopDispatchMask)
+ InterpFuncs[8]= cCmdInterpScalarBinop;
+ else
+ InterpFuncs[8]= cCmdInterpBinop;
+ if(clumpRecPtr->clumpScalarDispatchHints & scalarUnop2DispatchMask)
+ InterpFuncs[6]= cCmdInterpScalarUnop2;
+ else
+ InterpFuncs[6]= cCmdInterpUnop2;
+}
+
+NXT_STATUS cCmdActivateProgram(UBYTE * pFileName)
+{
+ UWORD i, j;
+ UBYTE * pCursor;
+
+ NXT_STATUS Status = NO_ERR;
+ PROG_FILE_OFFSETS FileOffsets;
+
+ LOADER_STATUS LStatus;
+ ULONG DataSize;
+ UBYTE * pData;
+ ULONG pDataHolder;
+ UWORD DefaultsOffset;
+
+ LStatus = pMapLoader->pFunc(OPENREADLINEAR, pFileName, (UBYTE*)(&pDataHolder), &DataSize);
+ pData = (UBYTE*)(pDataHolder);
+
+ //If Loader returned error or bad file pointer, bail out
+ if (LOADER_ERR(LStatus) != SUCCESS || pData == NULL || DataSize == 0)
+ return (ERR_FILE);
+
+ //Deactivate current program and re-initialize memory pool
+ cCmdDeactivateProgram();
+ cCmdInitPool();
+
+ //Stash this program's handle since we hold it open while running
+ VarsCmd.ActiveProgHandle = LOADER_HANDLE(LStatus);
+
+ //Stash this program's name for easy reference later
+ strncpy((PSZ)(VarsCmd.ActiveProgName), (PSZ)(pFileName), FILENAME_LENGTH + 1);
+
+ //Consume activation record data stream.
+ //See TargettingVIs/NXT.PackAR.vi for data stream packing details
+
+ //Read header portion of the file, calculating offsets and initializing VarsCmd
+ Status = cCmdReadFileHeader(pData, DataSize, &FileOffsets);
+ if (IS_ERR(Status))
+ return Status;
+
+ //Do some spot checks to make sure bad file contents didn't leave us with obviously insane VarsCmd contents
+ //!!! Should add alignment checks on these pointers to avoid data abort exceptions later
+ if (((UBYTE*)(VarsCmd.pCodespace) < pData)
+ || ((UBYTE*)(VarsCmd.pCodespace) >= (pData + DataSize))
+ || ((UBYTE*)(VarsCmd.pAllClumps) < POOL_START)
+ || ((UBYTE*)(VarsCmd.pAllClumps) >= POOL_SENTINEL)
+ || ((UBYTE*)(VarsCmd.pDataspace) < POOL_START)
+ || ((UBYTE*)(VarsCmd.pDataspace) >= POOL_SENTINEL)
+ || (VarsCmd.DataspaceSize == 0) )
+ {
+ NXT_BREAK;
+ return ERR_FILE;
+ }
+
+ //Initialize CLUMP_RECs as contiguous list in RAM
+ pCursor = (pData + FileOffsets.Clumps);
+ for (i = 0; i < VarsCmd.AllClumpsCount; i++)
+ {
+ CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
+ clumpPtr->InitFireCount = *(UBYTE*)(pCursor + i * VM_FILE_CLUMP_REC_SIZE);
+ clumpPtr->DependentCount = *(UBYTE*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 1);
+ clumpPtr->CodeStart = *(UWORD*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 2) + VarsCmd.pCodespace;
+
+ //Initialize remaining CLUMP_REC fields
+ clumpPtr->PC = clumpPtr->CodeStart;
+ clumpPtr->Link = NOT_A_CLUMP;
+
+ //Activate any clumps with CurrFireCount of 0
+ clumpPtr->CurrFireCount = clumpPtr->InitFireCount;
+ if (clumpPtr->CurrFireCount == 0)
+ cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)i);
+ }
+
+ //Patch up dependents in separate pass (reuse of pCursor)
+ pCursor += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;
+ for (i = 0; i < VarsCmd.AllClumpsCount; i++)
+ {
+ CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
+ if (clumpPtr->DependentCount > 0)
+ {
+ clumpPtr->pDependents = (CLUMP_ID*)(pCursor);
+
+ pCursor += (clumpPtr->DependentCount * sizeof(CLUMP_ID));
+ }
+ else
+ clumpPtr->pDependents = NULL;
+
+ //Patch up CodeEnd value based on CodeStart of next clump or last overall codeword
+ if (i < (VarsCmd.AllClumpsCount - 1))
+ clumpPtr->CodeEnd = (clumpPtr+1)->CodeStart - 1;
+ else
+ clumpPtr->CodeEnd = VarsCmd.CodespaceCount - 1 + VarsCmd.pCodespace;
+
+ //Test for empty/insane clump code definitions
+ NXT_ASSERT(clumpPtr->CodeStart < clumpPtr->CodeEnd);
+ }
+
+ // Check if the instructions within a clump are polymorphic and mark which table to dispatch from
+ for (i = 0; i < VarsCmd.AllClumpsCount; i++)
+ { // Check type on Boolean, math, ArrInit and ArrIndex, ingore GetSet I/O as these are always scalar
+ // do we need to check for DataArg encodings to I/O map??? GM
+ // Get Opcode and size of each instr, if ^^, check Arg types for Array or Cluster
+ CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
+ CODE_WORD *pInstr = clumpPtr->CodeStart, *lastPC = clumpPtr->CodeEnd;
+ ULONG InstrSize, opCode, shortOp, isT2Agg, isT3Agg, isScalarBinop= TRUE, isScalarUnop2= TRUE;
+ TYPE_CODE t1, t2, t3;
+ ULONG instrWord;
+ do
+ {
+ instrWord= *(UWORD*)pInstr;
+ opCode= OP_CODE(pInstr);
+ shortOp= (instrWord>>8) & 0x0F;
+ InstrSize = INSTR_SIZE(instrWord);
+ if (InstrSize == VAR_INSTR_SIZE)
+ InstrSize = ((UWORD*)pInstr)[1];
+ if(shortOp <= 7) // no shorts are binOps
+ {
+ t2= cCmdDSType(pInstr[2]);
+ isT2Agg= IS_AGGREGATE_TYPE(t2);
+ if(InstrSize == 8) {
+ t3= cCmdDSType(pInstr[3]);
+ isT3Agg= IS_AGGREGATE_TYPE(t3);
+ if(isT2Agg || isT3Agg) {
+ if(opCode == OP_CMP) {
+ UBYTE isString2, isString3;
+ isString2= (t2 == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[2])) == TC_UBYTE;
+ isString3= (t3 == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[3])) == TC_UBYTE;
+ t1= cCmdDSType(pInstr[1]);
+ if((!isString2 || !isString3) || t1 == TC_ARRAY) // allow strings to go scalar, don't let through element compares of bytes or Bools
+ isScalarBinop= FALSE;
+ }
+ else if(opCode == OP_BRCMP)
+ isScalarBinop= FALSE;
+ }
+ }
+ else if(InstrSize == 6 && isT2Agg && (opCode == OP_NOT || opCode == OP_BRTST))
+ isScalarUnop2= FALSE;
+ }
+ pInstr += InstrSize/2;
+ } while((isScalarBinop || isScalarUnop2) && pInstr < lastPC);
+ if(isScalarBinop)
+ clumpPtr->clumpScalarDispatchHints |= scalarBinopDispatchMask;
+ else
+ clumpPtr->clumpScalarDispatchHints &= ~scalarBinopDispatchMask;
+
+ if(isScalarUnop2)
+ clumpPtr->clumpScalarDispatchHints |= scalarUnop2DispatchMask;
+ else
+ clumpPtr->clumpScalarDispatchHints &= ~scalarUnop2DispatchMask;
+
+ }
+ //Programs with no active clumps constitutes an activation error
+ if (VarsCmd.RunQ.Head == NOT_A_CLUMP)
+ return (ERR_FILE);
+ else
+ {
+ // now that we know which clumps are scalar and poly, refresh dispatch table to match head
+ cCmdRefreshActiveClump(VarsCmd.RunQ.Head);
+
+ }
+
+ //Initialize dataspace with default values from file
+ //!!! This would be a good place to enforce check against potentially
+ // unsafe nested types (deeply nested types mean deep recursive calls)
+ DefaultsOffset = 0;
+ for (i = 0; i != NOT_A_DS_ID; i = cCmdNextDSElement(i))
+ {
+
+ Status = cCmdInflateDSDefaults(pData + FileOffsets.DSDefaults, &DefaultsOffset, i);
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ if ((DefaultsOffset != FileOffsets.DynamicDefaults)
+ || (DefaultsOffset + FileOffsets.DynamicDefaultsSize != FileOffsets.DSDefaultsSize))
+ {
+ NXT_BREAK;
+ return (ERR_FILE);
+ }
+
+ //Copy Dynamic defaults from file
+ memmove(VarsCmd.pDataspace + VarsCmd.DSStaticSize, pData + FileOffsets.DSDefaults + FileOffsets.DynamicDefaults, FileOffsets.DynamicDefaultsSize);
+
+ // fix memmgr links. old files contain unused backPtrs, we now use these to store backLink
+ DV_INDEX prev= NOT_A_DS_ID;
+ for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) {
+ DV_ARRAY[i].BackLink= prev;
+ prev= i;
+ }
+
+ //Verify the MemMgr ended up where we said it would
+ if ((UBYTE *)VarsCmd.MemMgr.pDopeVectorArray != VarsCmd.pDataspace + DV_ARRAY[0].Offset)
+ {
+ NXT_BREAK;
+ return (ERR_FILE);
+ }
+
+ //Initialize message queues
+ for (i = 0; i < MESSAGE_QUEUE_COUNT; i++)
+ {
+ VarsCmd.MessageQueues[i].ReadIndex = 0;
+ VarsCmd.MessageQueues[i].WriteIndex = 0;
+
+ for (j = 0; j < MESSAGES_PER_QUEUE; j++)
+ {
+ VarsCmd.MessageQueues[i].Messages[j] = NOT_A_DS_ID;
+ }
+ }
+
+ //Initialize datalog queue
+ VarsCmd.DatalogBuffer.ReadIndex = 0;
+ VarsCmd.DatalogBuffer.WriteIndex = 0;
+ for (j = 0; j < DATALOG_QUEUE_DEPTH; j++)
+ {
+ VarsCmd.DatalogBuffer.Datalogs[j] = NOT_A_DS_ID;
+ }
+
+ // now that we've loaded program, prime memmgr dopevectors based upon number of handles in ds.
+ ULONG numHandles= DV_ARRAY[0].Count/2;
+ if(numHandles > 200)
+ numHandles= 200;
+ Status = cCmdGrowDopeVectorArray(numHandles);
+
+ if (cCmdVerifyMemMgr() != TRUE)
+ return (ERR_FILE);
+
+ gUsageSemData= 0;
+ gRequestSemData= 0;
+ // preload all calibration coefficients into mem
+ cCmdLoadCalibrationFiles();
+ return (Status);
+}
+
+
+void cCmdDeactivateProgram()
+{
+ UBYTE i, tmp;
+
+ //Wipe away all references into the pool and clear all run-time data
+ VarsCmd.pCodespace = NULL;
+ VarsCmd.CodespaceCount = 0;
+
+ VarsCmd.pAllClumps = NULL;
+ VarsCmd.AllClumpsCount = 0;
+
+ VarsCmd.DataspaceCount = 0;
+ VarsCmd.pDataspaceTOC = NULL;
+ VarsCmd.pDataspace = NULL;
+ VarsCmd.DataspaceSize = 0;
+ VarsCmd.DSStaticSize = 0;
+
+ VarsCmd.MemMgr.Head = NOT_A_DS_ID;
+ VarsCmd.MemMgr.Tail = NOT_A_DS_ID;
+ VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;
+ VarsCmd.MemMgr.pDopeVectorArray = NULL;
+
+ VarsCmd.RunQ.Head = NOT_A_CLUMP;
+ VarsCmd.RunQ.Tail = NOT_A_CLUMP;
+
+ if (VarsCmd.ActiveProgHandle != NOT_A_HANDLE)
+ {
+ //Close handle that we've kept open for this program
+ pMapLoader->pFunc(CLOSE, &(VarsCmd.ActiveProgHandle), NULL, NULL);
+ VarsCmd.ActiveProgHandle = NOT_A_HANDLE;
+
+ //Clear internal stashed name
+ memset(VarsCmd.ActiveProgName, 0, FILENAME_LENGTH + 1);
+ }
+
+ //Close any files we had opened programatically
+ for (i = 0; i < MAX_HANDLES; i++)
+ {
+ //Copy i to tmp, because we pass a pointer to it to pFunc
+ tmp = i;
+ //Close file
+ if (*(VarsCmd.FileHandleTable[i]) != 0)
+ pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL);
+ }
+
+ //Clear FileHandleTable
+ memset(VarsCmd.FileHandleTable, 0, sizeof(VarsCmd.FileHandleTable));
+
+ return;
+}
+
+
+void cCmdResetDevices(void)
+{
+ UBYTE i;
+
+ //Clear NXT button counts so 'bumped' will work on first run
+ for (i = 0; i < NO_OF_BTNS; i++)
+ {
+ pMapButton->BtnCnt[i].RelCnt = 0;
+ //Need to clear short and long counts too, because RelCnt depends on them. No known side effects.
+ pMapButton->BtnCnt[i].ShortRelCnt = 0;
+ pMapButton->BtnCnt[i].LongRelCnt = 0;
+ }
+
+ for (i = 0; i < NO_OF_INPUTS; i++)
+ {
+ //Clear type and mode to defaults
+ pMapInput->Inputs[i].SensorType = NO_SENSOR;
+ pMapInput->Inputs[i].SensorMode = RAWMODE;
+
+ //Reset input values to 0 prior to running (clear things like stale rotation counts)
+ pMapInput->Inputs[i].ADRaw = 0;
+ pMapInput->Inputs[i].SensorRaw = 0;
+ pMapInput->Inputs[i].SensorValue = 0;
+
+ //Assert invalid data flag so future code is aware of these changes
+ pMapInput->Inputs[i].InvalidData = TRUE;
+ }
+
+ for (i = 0; i < NO_OF_OUTPUTS; i++)
+ {
+ //Coast and reset all motor parameters
+ pMapOutPut->Outputs[i].Mode = 0;
+ pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE;
+ pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE;
+ pMapOutPut->Outputs[i].Speed = 0;
+ pMapOutPut->Outputs[i].TachoLimit = 0;
+ pMapOutPut->Outputs[i].SyncTurnParameter = 0;
+ pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
+ }
+
+ //Lowspeed init, INSERT CODE !!!
+ for (i = 0; i < NO_OF_LOWSPEED_COM_CHANNEL; i++)
+ {
+ pMapLowSpeed->InBuf[i].InPtr = 0;
+ pMapLowSpeed->InBuf[i].OutPtr = 0;
+ pMapLowSpeed->InBuf[i].BytesToRx = 0;
+ pMapLowSpeed->OutBuf[i].InPtr = 0;
+ pMapLowSpeed->OutBuf[i].OutPtr = 0;
+ if (pMapLowSpeed->ChannelState[i] != LOWSPEED_IDLE)
+ {
+ pMapLowSpeed->ChannelState[i] = LOWSPEED_DONE;
+ pMapLowSpeed->State |= (0x01<<i);
+ }
+ }
+
+}
+
+
+//Add NewClump to end, updating Queue's head/tail as needed
+void cCmdEnQClump(CLUMP_Q * Queue, CLUMP_ID NewClump)
+{
+ //Make sure NewClump's ID is valid and not already on Q
+ NXT_ASSERT(cCmdIsClumpIDSane(NewClump));
+ NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
+ NXT_ASSERT(!cCmdIsClumpOnQ(Queue, NewClump));
+
+ VarsCmd.pAllClumps[NewClump].Link = NOT_A_CLUMP;
+
+ //If queue is empty, NewClump becomes both head and tail
+ if (Queue->Head == NOT_A_CLUMP)
+ {
+ NXT_ASSERT(Queue->Tail == NOT_A_CLUMP);
+
+ Queue->Head = NewClump;
+ Queue->Tail = NewClump;
+ if(Queue == &(VarsCmd.RunQ))
+ cCmdRefreshActiveClump(NewClump);
+ }
+ //Otherwise, tack onto the end
+ else
+ {
+ VarsCmd.pAllClumps[Queue->Tail].Link = NewClump;
+ Queue->Tail = NewClump;
+ }
+
+ return;
+}
+
+//Dequeue specified clump
+//Normal usage is to dequeue only from the head (i.e. pass Queue.Head as arg)
+void cCmdDeQClump(CLUMP_Q * Queue, CLUMP_ID Clump)
+{
+ CLUMP_ID CurrID, LinkID;
+
+ //Make sure Clump's ID is valid and is already on Queue
+ NXT_ASSERT(cCmdIsClumpIDSane(Clump));
+ NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
+ NXT_ASSERT(cCmdIsClumpOnQ(Queue, Clump));
+
+ CurrID = Queue->Head;
+
+ //If our clump is the head, move up the next and disconnect
+ if (CurrID == Clump)
+ {
+ Queue->Head = VarsCmd.pAllClumps[Clump].Link;
+ VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;
+
+ //If we just removed the last clump, patch up the queue's tail
+ if (Queue->Head == NOT_A_CLUMP)
+ Queue->Tail = NOT_A_CLUMP;
+ else if(Queue == &(VarsCmd.RunQ))
+ cCmdRefreshActiveClump(Queue->Head);
+ }
+ //Else, look through rest of list looking for a link to our clump
+ else
+ {
+ do
+ {
+ CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[CurrID];
+ LinkID = clumpPtr->Link;
+
+ //If we find a link to our clump, patch up predecessor's link
+ if (clumpPtr->Link == Clump)
+ {
+ clumpPtr->Link = VarsCmd.pAllClumps[Clump].Link;
+ VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;
+
+ //If we just removed the tail, patch tail
+ if (Clump == Queue->Tail)
+ Queue->Tail = CurrID;
+ }
+
+ CurrID = LinkID;
+ } while (CurrID != NOT_A_CLUMP);
+ }
+
+ return;
+}
+
+
+//Rotate head to tail and advance head for given Queue
+void cCmdRotateQ()
+{
+ CLUMP_ID CurrID;
+ CLUMP_REC * pClumpRec;
+ CLUMP_Q * Queue = &VarsCmd.RunQ;
+
+ //Make sure Queue is sane
+ NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
+
+ //If queue has at least two clumps
+ if (Queue->Head != Queue->Tail)
+ {
+ CurrID = Queue->Head;
+ pClumpRec = &(VarsCmd.pAllClumps[CurrID]);
+
+ //Disconnect head
+ Queue->Head = pClumpRec->Link;
+ pClumpRec->Link = NOT_A_CLUMP;
+
+ //Reconnect head as tail
+ pClumpRec = &(VarsCmd.pAllClumps[Queue->Tail]);
+ pClumpRec->Link = CurrID;
+ Queue->Tail = CurrID;
+
+ // reinit clump info
+ CurrID= Queue->Head;
+ cCmdRefreshActiveClump(Queue->Head);
+
+ //Make sure we didn't make any really stupid mistakes
+ NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
+ }
+
+ return;
+}
+
+
+UBYTE cCmdIsClumpOnQ(CLUMP_Q * Queue, CLUMP_ID Clump)
+{
+ CLUMP_ID CurrID;
+
+ //Make sure Clump's ID is valid and is already on Queue
+ NXT_ASSERT(cCmdIsClumpIDSane(Clump));
+ NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
+
+ CurrID = Queue->Head;
+
+ while (CurrID != NOT_A_CLUMP)
+ {
+ if (CurrID == Clump)
+ return TRUE;
+
+ CurrID = VarsCmd.pAllClumps[CurrID].Link;
+ }
+
+ return FALSE;
+}
+
+
+UBYTE cCmdIsQSane(CLUMP_Q * Queue)
+{
+ CLUMP_ID Head, Tail;
+ CLUMP_REC * pHead;
+
+ if (Queue == NULL)
+ {
+ NXT_BREAK;
+ return FALSE;
+ }
+
+ Head = Queue->Head;
+ Tail = Queue->Tail;
+
+ if (Head == NOT_A_CLUMP && cCmdIsClumpIDSane(Tail))
+ return FALSE;
+
+ if (cCmdIsClumpIDSane(Head) && Tail == NOT_A_CLUMP)
+ return FALSE;
+
+ if (cCmdIsClumpIDSane(Head) && cCmdIsClumpIDSane(Tail))
+ {
+ pHead = &(VarsCmd.pAllClumps[Head]);
+
+ //!!! More comprehensive queue tests could go here
+
+ //Check for mislinked head if there are at least two queue members
+ if (Head != Tail && pHead->Link == NOT_A_CLUMP)
+ return FALSE;
+ }
+
+ return TRUE;
+}
+
+//
+// Mutex queuing functions
+//
+
+NXT_STATUS cCmdAcquireMutex(MUTEX_Q * Mutex)
+{
+ NXT_STATUS Status = NO_ERR;
+ CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below
+
+ NXT_ASSERT(Mutex != NULL && cCmdIsClumpIDSane(Clump));
+
+ if (Mutex->Owner == NOT_A_CLUMP)
+ {
+ //Mutex is open, so just take it
+ Mutex->Owner = Clump;
+
+ NXT_ASSERT(Mutex->WaitQ.Head == NOT_A_CLUMP && Mutex->WaitQ.Tail == NOT_A_CLUMP);
+ }
+ else
+ {
+ //Mutex is reserved by someone else, take self off RunQ and add to WaitQ
+ cCmdDeQClump(&(VarsCmd.RunQ), Clump);
+ cCmdEnQClump(&(Mutex->WaitQ), Clump);
+ Status = CLUMP_SUSPEND;
+ }
+
+ NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));
+
+ return (Status);
+}
+
+
+NXT_STATUS cCmdReleaseMutex(MUTEX_Q * Mutex)
+{
+#if WIN_DEBUG || defined(ARM_DEBUG)
+ CLUMP_ID Clump= VarsCmd.RunQ.Head;
+#endif
+ NXT_ASSERT(Mutex != NULL);
+ //!!! don't actually need to pass in Owner clump, but provides nice error checking for now
+ // Might want to return an error/warning if we see a Release on an free mutex, though...
+ NXT_ASSERT(Clump != NOT_A_CLUMP && Mutex->Owner == Clump);
+
+ //Always set new Owner to WaitQ's Head, since NOT_A_CLUMP means mutex is free
+ Mutex->Owner = Mutex->WaitQ.Head;
+
+ if (Mutex->Owner != NOT_A_CLUMP)
+ {
+ cCmdDeQClump(&(Mutex->WaitQ), Mutex->Owner);
+ cCmdEnQClump(&(VarsCmd.RunQ), Mutex->Owner);
+ }
+
+ NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));
+ NXT_ASSERT(cCmdIsQSane(&(VarsCmd.RunQ)));
+
+ return (NO_ERR);
+}
+
+// No instruction to do this yet, but put current clump to sleep until awakeTime occurs
+NXT_STATUS cCmdSleepClump(ULONG time)
+{
+ CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below
+ CLUMP_REC * pClump = &(VarsCmd.pAllClumps[Clump]);
+ cCmdDeQClump(&(VarsCmd.RunQ), Clump);
+ cCmdEnQClump(&(VarsCmd.RestQ), Clump);
+ pClump->awakenTime= time;
+ return CLUMP_SUSPEND;
+}
+
+UBYTE cCmdCheckRestQ(ULONG currTime)
+{
+ UBYTE awakened= FALSE;
+ CLUMP_ID curr, next;
+ CLUMP_REC * pClump;
+ curr= VarsCmd.RestQ.Head;
+ while(curr != NOT_A_CLUMP) {
+ pClump= &(VarsCmd.pAllClumps[curr]);
+ next= pClump->Link;
+ if(pClump->awakenTime <= currTime) {
+ pClump->awakenTime= 0; // not necessary, but for debugging identification
+ cCmdDeQClump(&(VarsCmd.RestQ), curr);
+ cCmdEnQClump(&(VarsCmd.RunQ), curr);
+ awakened= TRUE;
+ }
+ curr= next;
+ }
+ return awakened;
+}
+
+NXT_STATUS cCmdSchedDependents(CLUMP_ID Clump, SWORD Begin, SWORD End)
+{
+ CLUMP_ID CurrDepClumpID;
+ SWORD i;
+
+ //Begin and End specify range of CLUMP_IDs in dependent list to schedule
+ //If either equals -1, both should equal -1, and no dependents will be scheduled
+ //Else schedule specified subset offset from pDependents
+
+ //Check for valid args
+ NXT_ASSERT(cCmdIsClumpIDSane(Clump));
+ NXT_ASSERT((Begin >= 0 && End >= 0 && End < VarsCmd.pAllClumps[Clump].DependentCount)
+ || (Begin == -1 && End == -1));
+
+ //If non-empty range
+ if (Begin != -1 || End != -1)
+ {
+ //update dependents, scheduling if their CurrFireCount reaches 0
+ for (i = Begin; i <= End; i++)
+ {
+ CurrDepClumpID = VarsCmd.pAllClumps[Clump].pDependents[i];
+
+ NXT_ASSERT(cCmdIsClumpIDSane(CurrDepClumpID));
+
+ VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount--;
+
+ if (VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount == 0)
+ cCmdEnQClump(&(VarsCmd.RunQ), CurrDepClumpID);
+ }
+ }
+
+ return (NO_ERR);
+}
+
+
+NXT_STATUS cCmdSchedDependent(CLUMP_ID Clump, CLUMP_ID TargetClump)
+{
+ //TargetClump specifies the clump number of the target to schedule explicitly.
+
+ //Check for valid args
+ NXT_ASSERT(cCmdIsClumpIDSane(Clump));
+ NXT_ASSERT(cCmdIsClumpIDSane(TargetClump));
+
+ CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[TargetClump];
+ clumpPtr->CurrFireCount--;
+ if (clumpPtr->CurrFireCount == 0)
+ cCmdEnQClump(&(VarsCmd.RunQ), TargetClump);
+
+ return (NO_ERR);
+}
+
+
+UBYTE cCmdIsClumpIDSane(CLUMP_ID Clump)
+{
+ if (Clump < VarsCmd.AllClumpsCount)
+ return TRUE;
+ else
+ return FALSE;
+}
+
+
+//
+// Memory pool management functions
+//
+void cCmdInitPool(void)
+{
+ ULONG i;
+ ULONG *poolPtr;
+
+ //VarsCmd.Pool is a UBYTE pointer to ULONG array
+ //This was done to enforce portable alignment.
+ VarsCmd.Pool = (UBYTE*)(IOMapCmd.MemoryPool);
+
+ for (i = (POOL_MAX_SIZE / 4), poolPtr= (ULONG*)&(POOL_START)[0]; i>0; i--, poolPtr++)
+ *poolPtr = 0xDEADBEEF;
+
+ VarsCmd.PoolSize = 0;
+}
+
+
+#if VMProfilingCode
+ULONG memMgrTime= 0;
+#endif
+NXT_STATUS cCmdDSArrayAlloc(DS_ELEMENT_ID DSElementID, UWORD Offset, UWORD NewCount)
+{
+ NXT_STATUS Status = NO_ERR;
+ UWORD DVIndex;
+ UWORD OldCount;
+ UWORD i;
+#if VMProfilingCode
+ ULONG enterTime= dTimerReadHiRes();
+#endif
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+
+ //Only arrays are valid here
+ //!!! Recommended to upgrade NXT_ASSERT to ERR_INSTR return
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);
+
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+ OldCount = DV_ARRAY[DVIndex].Count;
+
+ if(OldCount == NewCount)
+ goto allocExit;
+ Status = cCmdDVArrayAlloc(DVIndex, NewCount);
+
+ if (Status < NO_ERR)
+ goto allocExit;
+
+ if(!IS_AGGREGATE_TYPE(cCmdDSType(INC_ID(DSElementID))))
+ goto allocExit;
+
+ if (OldCount > NewCount)
+ {
+ //Free dope vectors for sub-arrays.
+ for (i = NewCount; i < OldCount; i++)
+ {
+ Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
+ if (IS_ERR(Status))
+ goto allocExit;
+ }
+ }
+ else if (OldCount < NewCount)
+ {
+ //Alloc dope vectors for sub-arrays. Set up DVIndexes
+ for (i = OldCount; i < NewCount; i++)
+ {
+ Status = cCmdAllocSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
+ if (IS_ERR(Status))
+ goto allocExit;
+ }
+ }
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+allocExit:
+#if VMProfilingCode
+ memMgrTime += dTimerReadHiRes() - enterTime;
+#endif
+ return Status;
+}
+
+NXT_STATUS cCmdDVArrayAlloc(DV_INDEX DVIndex, UWORD NewCount)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE *pData;
+ UWORD ArraySize, InplaceSize;
+ UWORD NextDVIndex;
+ UWORD OldCount;
+
+ OldCount = DV_ARRAY[DVIndex].Count;
+
+ if (OldCount == NewCount)
+ {
+ //Nothing to alloc. Return.
+ return Status;
+ }
+ else if (OldCount > NewCount)
+ {
+ //Already have the space. Shrink inplace.
+ DV_ARRAY[DVIndex].Count = NewCount;
+ return Status;
+ }
+ else // need to grow array
+ {
+ //Calculate new array size
+ ArraySize = NewCount * DV_ARRAY[DVIndex].ElemSize;
+
+ //Try growing inplace
+ // If the Offset == NOT_AN_OFFSET then the array has never been allocated and can't grow inplace.
+ if (DV_ARRAY[DVIndex].Offset != NOT_AN_OFFSET)
+ {
+ //Get pointer to next dope vector in dataspace
+ if (DV_ARRAY[DVIndex].Link != NOT_A_DS_ID)
+ {
+ NextDVIndex = DV_ARRAY[DVIndex].Link;
+ InplaceSize = DV_ARRAY[NextDVIndex].Offset - DV_ARRAY[DVIndex].Offset;
+ }
+ else
+ {
+ //Last element in dataspace.
+ NXT_ASSERT(DVIndex == VarsCmd.MemMgr.Tail);
+ InplaceSize = VarsCmd.DataspaceSize - DV_ARRAY[DVIndex].Offset;
+ }
+
+ if (ArraySize <= InplaceSize)
+ {
+ DV_ARRAY[DVIndex].Count = NewCount;
+ return Status;
+ }
+ }
+
+ //Can't grow inplace, have to allocate new space
+
+ //Make sure we properly align for type
+ //!!! This could also overflow memory (make PoolSize > POOL_MAX_SIZE) if we're within 3 bytes of the end.
+ // I don't think it matters because if it does happend, we'll trigger the ERR_MEM below and compact.
+ // During compaction, we'll reclaim these unused bytes.
+ //!!! Aligning beginning of ALL arrays to 4 byte address
+ ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG);
+ ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG);
+
+ if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE)
+ {
+ //Not enough memory available
+ return ERR_MEM;
+ }
+
+ //Get data from end of pool
+ pData = VarsCmd.Pool + VarsCmd.PoolSize;
+ //Grow pool and dataspace
+ VarsCmd.PoolSize += ArraySize;
+ VarsCmd.DataspaceSize += ArraySize;
+
+ //Move old Array Data to new allocation
+ if(OldCount)
+ memmove(pData, VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
+ //!!! Clear mem so old mem doesn't contain stale data. Not strictly needed.
+#if WIN_DEBUG || defined(ARM_DEBUG)
+ memset(VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, 0xFF, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
+#endif
+ //Update dope vector
+ DV_ARRAY[DVIndex].Offset = pData - VarsCmd.pDataspace;
+ DV_ARRAY[DVIndex].Count = NewCount;
+
+ //Move dope vector to end of MemMgr list
+ Status = cCmdMemMgrMoveToTail(DVIndex);
+ if (IS_ERR(Status))
+ return Status;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+ }
+
+ return Status;
+}
+
+
+//!!! Recursive function
+NXT_STATUS cCmdAllocSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ // Walks a single array element to see if it contains arrays
+ // For any array it finds, a dope vector is allocated and the DVIndex is placed in the dataspace for the parent array.
+ // This is a non-recursive function. It only walks the immediate array element.
+ // DSElementID - ID of array sub-entry
+ // Offset - offset to array element in dataspace
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode;
+ DV_INDEX DVIndex;
+ UWORD i;
+ UWORD DVIndexOffset; //Offset to DVIndex field that points to the DopeVector from pDataspace
+ UWORD LoopCount = 1;
+ UWORD ElemSize;
+
+ for (i = 0; i < LoopCount; i++)
+ {
+ TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i));
+ if (TypeCode == TC_CLUSTER)
+ {
+ LoopCount += cCmdClusterCount(DSElementID);
+ }
+ else if (TypeCode == TC_ARRAY)
+ {
+ //!!! ElemSize is a static value, but we don't have anywhere we put it (another TOC sub-entry?)
+ // It'd be nice to not have to recalculate it.
+ ElemSize = cCmdCalcArrayElemSize((DS_ELEMENT_ID)(DSElementID + i));
+ DVIndexOffset = VarsCmd.pDataspaceTOC[DSElementID + i].DSOffset + Offset;
+ Status = cCmdAllocDopeVector(&DVIndex, ElemSize);
+ if (IS_ERR(Status))
+ return Status;
+
+ *((UWORD *)(VarsCmd.pDataspace + DVIndexOffset)) = DVIndex;
+ }
+ }
+
+ return Status;
+}
+
+
+//!!! Recursive function
+NXT_STATUS cCmdFreeSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ // Walks a single array element to see if it contains arrays
+ // Frees all dope vectors associated with the array element.
+ // Recursively deletes sub-arrays.
+ // DSElementID - ID of array sub-entry
+ // Offset - offset to array element in dataspace
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode;
+ DV_INDEX DVIndex;
+ UWORD i, Count;
+
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (TypeCode == TC_ARRAY)
+ {
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+
+ NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);
+
+ Count = DV_ARRAY[DVIndex].Count;
+ //Recur on sub-elements
+ for (i = 0; i < Count; i++)
+ {
+ Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ //Free Dope Vector
+ Status = cCmdFreeDopeVector(DVIndex);
+ }
+ else if (TypeCode == TC_CLUSTER)
+ {
+ Count = cCmdClusterCount(DSElementID);
+ DSElementID = INC_ID(DSElementID);
+ //Recur on sub-elements
+ for (i = 0; i < Count; i++)
+ {
+ Status = cCmdFreeSubArrayDopeVectors((DS_ELEMENT_ID)(DSElementID + i), Offset);
+ if (IS_ERR(Status))
+ return Status;
+ }
+ }
+
+ return Status;
+}
+
+
+NXT_STATUS cCmdAllocDopeVector(DV_INDEX *pIndex, UWORD ElemSize)
+{
+ NXT_STATUS Status = NO_ERR;
+
+ if (VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
+ {
+ //No free DVs. Need to grow DopeVector array.
+ Status = cCmdGrowDopeVectorArray(DV_ARRAY_GROWTH_COUNT);
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ if(VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
+ return ERR_MEM;
+
+ //Remove DV from free list
+ *pIndex = VarsCmd.MemMgr.FreeHead;
+ VarsCmd.MemMgr.FreeHead = DV_ARRAY[*pIndex].Link;
+ if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
+ DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= NOT_A_DS_ID;
+ //Add DV to tail of MemMgr list
+ Status = cCmdMemMgrInsertAtTail(*pIndex);
+
+ //Initialize values
+ DV_ARRAY[*pIndex].Offset = NOT_AN_OFFSET;
+ DV_ARRAY[*pIndex].ElemSize = ElemSize;
+ DV_ARRAY[*pIndex].Count = 0;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ return Status;
+}
+
+//
+//cCmdFreeDopeVector() - Open up a spot in the DopeVectorArray for future use
+// The DopeVectorArray doesn't shrink when arrays (and their dope vectors) are deleted.
+// Instead they're pushed on the free list and the array stays the same size.
+// Future allocations check the free list before resorting to cCmdGrowDopeVectorArray()
+//
+NXT_STATUS cCmdFreeDopeVector(DV_INDEX DVIndex)
+{
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX prev, post;
+
+ //Bounds check
+ NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);
+
+ //Reset dope vector fields
+ DV_ARRAY[DVIndex].Count = 0;
+ DV_ARRAY[DVIndex].ElemSize = 0;
+ DV_ARRAY[DVIndex].Offset = NOT_AN_OFFSET;
+
+ //Remove from MemMgr list
+ if (DVIndex == VarsCmd.MemMgr.Head)
+ {
+ VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link;
+ if(VarsCmd.MemMgr.Head != NOT_A_DS_ID)
+ DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
+ }
+ else
+ {
+ // patchup middle or end of list.
+ prev= DV_ARRAY[DVIndex].BackLink;
+ post= DV_ARRAY[DVIndex].Link;
+ NXT_ASSERT(prev != NOT_A_DS_ID);
+
+ DV_ARRAY[prev].Link = post;
+ if(post != NOT_A_DS_ID)
+ DV_ARRAY[post].BackLink= prev;
+ if (DVIndex == VarsCmd.MemMgr.Tail)
+ VarsCmd.MemMgr.Tail = prev;
+ //Make sure we found the previous DV, otherwise this DV was not in the the list (already freed?)
+ }
+
+ //Push onto free list
+ DV_ARRAY[DVIndex].Link = VarsCmd.MemMgr.FreeHead;
+ DV_ARRAY[DVIndex].BackLink = NOT_A_DS_ID;
+ DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= DVIndex;
+ VarsCmd.MemMgr.FreeHead = DVIndex;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ return Status;
+}
+
+//
+//cCmdGrowDopeVectorArray() - expand DopeVectorArray to be able to track more dataspace arrays
+//
+NXT_STATUS cCmdGrowDopeVectorArray(UWORD NewNodesCount)
+{
+ NXT_STATUS Status = NO_ERR;
+ UWORD ArraySize;
+ UWORD OldCount, NewCount, i;
+ UBYTE * pData;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ OldCount = DV_ARRAY[0].Count;
+ NewCount = OldCount + NewNodesCount;
+ NXT_ASSERT(NewCount > OldCount);
+
+ ArraySize = DV_ARRAY[0].ElemSize * NewCount;
+
+ //!!! Aligning beginning of ALL arrays to 4 byte address
+ ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG);
+ ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG);
+
+ if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE)
+ {
+ //Not enough memory available
+ return ERR_MEM;
+ }
+
+ //Get data from end of pool
+ pData = VarsCmd.Pool + VarsCmd.PoolSize;
+ //Grow pool and dataspace
+ VarsCmd.PoolSize += ArraySize;
+ VarsCmd.DataspaceSize += ArraySize;
+
+ //Move DopeVector Array
+ memmove(pData, (UBYTE *)VarsCmd.MemMgr.pDopeVectorArray, (UWORD)(DV_ARRAY[0].ElemSize * DV_ARRAY[0].Count));
+
+ //Update MemMgr pointer
+ VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)pData;
+ IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
+
+ //Update dope vector
+ DV_ARRAY[0].Offset = pData - VarsCmd.pDataspace;
+ DV_ARRAY[0].Count = NewCount;
+
+ //Add new DopeVectors to free list
+ //Push in reverse order so they get popped in order (mostly for ease of debugging)
+ for (i = NewCount - 1; i >= OldCount; i--)
+ {
+ DV_ARRAY[i].Offset = 0xFFFF;
+ DV_ARRAY[i].ElemSize = 0;
+ DV_ARRAY[i].Count = 0;
+ DV_ARRAY[i].BackLink = NOT_A_DS_ID;
+ if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
+ DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink = i;
+ DV_ARRAY[i].Link = VarsCmd.MemMgr.FreeHead;
+ VarsCmd.MemMgr.FreeHead = i;
+ }
+
+ //Move dope vector to end of MemMgr list
+ Status = cCmdMemMgrMoveToTail(0);
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ return Status;
+}
+
+
+UWORD cCmdCalcArrayElemSize(DS_ELEMENT_ID DSElementID)
+{
+ TYPE_CODE TypeCode;
+ UWORD SizeOfType;
+ UWORD i;
+ UWORD LoopCount = 1;
+ UWORD Size = 0;
+ UWORD Alignment = 0;
+
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);
+
+ DSElementID = INC_ID(DSElementID);
+ for (i = 0; i < LoopCount; i++)
+ {
+ TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i));
+ if (TypeCode == TC_CLUSTER)
+ {
+ LoopCount += cCmdClusterCount((DS_ELEMENT_ID)(DSElementID + i));
+ }
+ else
+ {
+ SizeOfType = cCmdSizeOf(TypeCode);
+ ALIGN_TO_MOD(Size, SizeOfType);
+ Size += SizeOfType;
+ if (SizeOfType > Alignment)
+ Alignment = SizeOfType;
+ }
+ }
+ ALIGN_TO_MOD(Size, Alignment);
+
+ return Size;
+}
+
+
+NXT_STATUS cCmdMemMgrMoveToTail(DV_INDEX DVIndex)
+{
+ DV_INDEX prev, post;
+
+ //Bounds check
+ NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);
+
+ //Short circut if its already at the tail
+ if (DVIndex == VarsCmd.MemMgr.Tail)
+ return NO_ERR;
+
+ if (DVIndex == VarsCmd.MemMgr.Head) {
+ VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link;
+ DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
+ }
+ else
+ {
+ // connect to middle or end of list.
+ prev= DV_ARRAY[DVIndex].BackLink;
+ post= DV_ARRAY[DVIndex].Link;
+ NXT_ASSERT(prev != NOT_A_DS_ID);
+ DV_ARRAY[prev].Link = post;
+ if(post != NOT_A_DS_ID)
+ DV_ARRAY[post].BackLink= prev;
+ }
+
+ DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
+ DV_ARRAY[DVIndex].BackLink = VarsCmd.MemMgr.Tail;
+ if(VarsCmd.MemMgr.Tail != NOT_A_DS_ID)
+ DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex;
+ VarsCmd.MemMgr.Tail = DVIndex;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ return NO_ERR;
+}
+
+
+NXT_STATUS cCmdMemMgrInsertAtTail(DV_INDEX DVIndex)
+{
+ //Bounds check
+ NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);
+
+ DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex;
+ DV_ARRAY[DVIndex].BackLink= VarsCmd.MemMgr.Tail;
+ DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
+ VarsCmd.MemMgr.Tail = DVIndex;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ return NO_ERR;
+}
+
+
+UBYTE cCmdVerifyMemMgr()
+{
+ DV_INDEX i, prev, post;
+ UWORD CurrOffset = 0;
+ UWORD PrevOffset = 0;
+ UWORD DVCount = 0;
+
+ //Make sure the MemMgr list is properly sorted in ascending offset order
+ for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link)
+ {
+ CurrOffset = DV_ARRAY[i].Offset;
+
+ if (CurrOffset != 0xFFFF)
+ {
+ if (PrevOffset > CurrOffset)
+ return FALSE;
+
+ PrevOffset = CurrOffset;
+ }
+
+ prev= DV_ARRAY[i].BackLink;
+ post= DV_ARRAY[i].Link;
+ if (post == NOT_A_DS_ID && i != VarsCmd.MemMgr.Tail)
+ return FALSE;
+ else if(prev == NOT_A_DS_ID && i != VarsCmd.MemMgr.Head)
+ return FALSE;
+ else if(prev != NOT_A_DS_ID && DV_ARRAY[prev].Link != i)
+ return FALSE;
+ else if(post != NOT_A_DS_ID && DV_ARRAY[post].BackLink != i)
+ return FALSE;
+
+ DVCount++;
+ }
+
+ // could check link and backlinks too
+ for (i = VarsCmd.MemMgr.FreeHead; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link)
+ {
+ DVCount++;
+ }
+
+ //Make sure the # of dope vectors = # used + # free
+ if (DVCount != DV_ARRAY[0].Count)
+ return FALSE;
+
+ return TRUE;
+}
+
+
+NXT_STATUS cCmdDSCompact(void)
+{
+ NXT_STATUS Status = NO_ERR;
+
+ DV_INDEX CurrIndex;
+ UWORD NewOffset;
+ UWORD CurrOffset;
+ UWORD Size;
+ UWORD DeltaDSSize;
+ UWORD TempOffset, TempSize;
+
+#if VM_BENCHMARK
+ ULONG StartTime, TotalTime;
+
+ VarsCmd.CompactionCount++;
+ VarsCmd.LastCompactionTick = IOMapCmd.Tick - VarsCmd.StartTick;
+
+ StartTime = dTimerRead();
+#endif
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+ NewOffset = VarsCmd.DSStaticSize;
+ for (CurrIndex = VarsCmd.MemMgr.Head; CurrIndex != NOT_A_DS_ID; CurrIndex = DV_ARRAY[CurrIndex].Link)
+ {
+ //Align NewOffset for array to 4 byte address.
+ ALIGN_TO_MOD(NewOffset, SIZE_ULONG);
+
+ CurrOffset = DV_ARRAY[CurrIndex].Offset;
+ if (CurrOffset != NOT_AN_OFFSET)
+ {
+ Size = DV_ARRAY[CurrIndex].ElemSize * DV_ARRAY[CurrIndex].Count;
+ if (CurrOffset != NewOffset)
+ {
+ NXT_ASSERT(NewOffset < CurrOffset);
+ memmove(VarsCmd.pDataspace + NewOffset, VarsCmd.pDataspace + CurrOffset, Size);
+
+ // Clear mem to make stale data references more obvious while debugging.
+ // Correct for overlapping memory regions (make sure we don't clear what we just moved).
+ //!!! Clearing step not strictly necessary, so it could be optimized out
+ if (NewOffset + Size > CurrOffset)
+ {
+ TempOffset = NewOffset + Size;
+ TempSize = Size - (TempOffset - CurrOffset);
+ }
+ else
+ {
+ TempOffset = CurrOffset;
+ TempSize = Size;
+ }
+ memset(VarsCmd.pDataspace + TempOffset, 0xFF, TempSize);
+
+ //Update pDopeVectorArray if we move the dope vector array
+ if (CurrIndex == 0)
+ {
+ VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + NewOffset);
+ IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
+ }
+
+ //Update offset in DV Array
+ DV_ARRAY[CurrIndex].Offset = NewOffset;
+ }
+
+ NewOffset += Size;
+ }
+ }
+
+ DeltaDSSize = VarsCmd.DataspaceSize - NewOffset;
+
+ VarsCmd.PoolSize -= DeltaDSSize;
+ VarsCmd.DataspaceSize -= DeltaDSSize;
+
+ NXT_ASSERT(cCmdVerifyMemMgr());
+
+#if VM_BENCHMARK
+ TotalTime = dTimerRead() - StartTime;
+
+ if (TotalTime > VarsCmd.MaxCompactionTime)
+ VarsCmd.MaxCompactionTime = TotalTime;
+#endif
+
+ return Status;
+}
+
+
+//
+// Message Queue functions
+//
+
+NXT_STATUS cCmdMessageWrite(UWORD QueueID, UBYTE * pData, UWORD Length)
+{
+ NXT_STATUS Status = NO_ERR;
+
+ if (pData == NULL)
+ return ERR_ARG;
+
+ if (QueueID >= MESSAGE_QUEUE_COUNT)
+ return ERR_INVALID_QUEUE;
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ return ERR_NO_PROG;
+
+ //Can't accept oversize messages because we treat them as strings (truncation would remove null termination)
+ if (Length > MAX_MESSAGE_SIZE)
+ return ERR_INVALID_SIZE;
+
+ if (IS_DV_INDEX_SANE(GET_WRITE_MSG(QueueID)))
+ {
+ //A message is already there, the queue is full
+ NXT_ASSERT(VarsCmd.MessageQueues[QueueID].WriteIndex == VarsCmd.MessageQueues[QueueID].ReadIndex);
+
+ //Bump read index, drop existing message to make room for our new incoming message
+ VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE;
+ }
+ else
+ {
+ //Allocate dope vector for message
+ Status = cCmdAllocDopeVector(&GET_WRITE_MSG(QueueID), 1);
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ //Allocate storage for message
+ Status = cCmdDVArrayAlloc(GET_WRITE_MSG(QueueID), Length);
+ if (IS_ERR(Status))
+ {
+ //Clear the dope vector for the message, since we're unable to put a message there.
+ cCmdFreeDopeVector(GET_WRITE_MSG(QueueID));
+ SET_WRITE_MSG(QueueID, NOT_A_DS_ID);
+ return Status;
+ }
+
+ //Copy message
+ memmove(cCmdDVPtr(GET_WRITE_MSG(QueueID)), pData, Length);
+
+ //Advance write index
+ VarsCmd.MessageQueues[QueueID].WriteIndex = (VarsCmd.MessageQueues[QueueID].WriteIndex + 1) % MESSAGES_PER_QUEUE;
+
+ return Status;
+}
+
+
+NXT_STATUS cCmdMessageGetSize(UWORD QueueID, UWORD * Size)
+{
+ DV_INDEX ReadDVIndex;
+
+ if (Size == NULL)
+ return (ERR_ARG);
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ {
+ *Size = 0;
+ return (ERR_NO_PROG);
+ }
+
+ if (QueueID >= MESSAGE_QUEUE_COUNT)
+ {
+ *Size = 0;
+ return (ERR_INVALID_QUEUE);
+ }
+
+ ReadDVIndex = GET_READ_MSG(QueueID);
+
+ if (IS_DV_INDEX_SANE(ReadDVIndex))
+ {
+ *Size = (DV_ARRAY[ReadDVIndex].Count);
+ return (NO_ERR);
+ }
+ else
+ {
+ *Size = 0;
+ return (STAT_MSG_EMPTY_MAILBOX);
+ }
+}
+
+
+NXT_STATUS cCmdMessageRead(UWORD QueueID, UBYTE * pBuffer, UWORD Length, UBYTE Remove)
+{
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX ReadDVIndex;
+
+ if (pBuffer == NULL)
+ return (ERR_ARG);
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ return (ERR_NO_PROG);
+
+ if (QueueID >= MESSAGE_QUEUE_COUNT)
+ return (ERR_INVALID_QUEUE);
+
+ ReadDVIndex = GET_READ_MSG(QueueID);
+
+ if (IS_DV_INDEX_SANE(ReadDVIndex))
+ {
+ //If Buffer doesn't have room for the entire message,
+ //don't risk incomplete string floating around
+ if (Length < DV_ARRAY[ReadDVIndex].Count)
+ return (ERR_INVALID_SIZE);
+
+ //Copy message
+ memmove(pBuffer, cCmdDVPtr(ReadDVIndex), DV_ARRAY[ReadDVIndex].Count);
+
+ if (Remove)
+ {
+ //Free memory used by message
+ Status = cCmdFreeDopeVector(ReadDVIndex);
+ if (IS_ERR(Status))
+ return Status;
+
+ SET_READ_MSG(QueueID, NOT_A_DS_ID);
+
+ //Advance read index
+ VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE;
+ }
+ }
+ else
+ {
+ //No message to read, message Queue is empty
+ NXT_ASSERT(VarsCmd.MessageQueues[QueueID].ReadIndex == VarsCmd.MessageQueues[QueueID].WriteIndex);
+
+ return (STAT_MSG_EMPTY_MAILBOX);
+ }
+
+ return Status;
+}
+
+//
+// Datalog Queue function(s)
+//
+
+NXT_STATUS cCmdDatalogWrite(UBYTE * pData, UWORD Length)
+{
+ NXT_STATUS Status = NO_ERR;
+
+ if (pData == NULL)
+ return ERR_ARG;
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ return (ERR_NO_PROG);
+
+ //Can't accept oversize messages because we treat them as strings (truncation would remove null termination)
+ if (Length > MAX_DATALOG_SIZE)
+ return ERR_INVALID_SIZE;
+
+ if (IS_DV_INDEX_SANE(GET_WRITE_DTLG()))
+ {
+ //A message is already there, the queue is full
+ NXT_ASSERT(VarsCmd.DatalogBuffer.WriteIndex == VarsCmd.DatalogBuffer.ReadIndex);
+ Status = STAT_MSG_BUFFERWRAP;
+ //Bump read index, drop existing message to make room for our newly acquired datalog
+ VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
+ }
+ else
+ {
+ //Allocate dope vector for message
+ Status = cCmdAllocDopeVector(&GET_WRITE_DTLG(), 1);
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ //Allocate storage for message
+ Status |= cCmdDVArrayAlloc(GET_WRITE_DTLG(), Length);
+ if (IS_ERR(Status))
+ {
+ //Clear the dope vector for the message, since we're unable to put a message there.
+ cCmdFreeDopeVector(GET_WRITE_DTLG());
+ SET_WRITE_DTLG(NOT_A_DS_ID);
+ return Status;
+ }
+
+ //Copy message
+ memmove(cCmdDVPtr(GET_WRITE_DTLG()), pData, Length);
+
+ //Advance write index
+ VarsCmd.DatalogBuffer.WriteIndex = (VarsCmd.DatalogBuffer.WriteIndex + 1) % DATALOG_QUEUE_DEPTH;
+
+ return Status;
+}
+
+NXT_STATUS cCmdDatalogGetSize(UWORD * Size)
+{
+ DV_INDEX ReadDVIndex;
+
+ if (Size == NULL)
+ return (ERR_ARG);
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ {
+ *Size = 0;
+ return (ERR_NO_PROG);
+ }
+
+ ReadDVIndex = GET_READ_DTLG();
+
+ if (IS_DV_INDEX_SANE(ReadDVIndex))
+ {
+ *Size = (DV_ARRAY[ReadDVIndex].Count);
+ return (NO_ERR);
+ }
+ else
+ {
+ *Size = 0;
+ return (STAT_MSG_EMPTY_MAILBOX);
+ }
+}
+
+NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove)
+{
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX ReadDVIndex;
+
+ if (pBuffer == NULL)
+ return (ERR_ARG);
+
+ if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
+ return (ERR_NO_PROG);
+
+ ReadDVIndex = GET_READ_DTLG();
+
+ if (IS_DV_INDEX_SANE(ReadDVIndex))
+ {
+ //If Buffer doesn't have room for the entire message,
+ //don't risk incomplete string floating around
+ if (Length < DV_ARRAY[ReadDVIndex].Count)
+ return (ERR_INVALID_SIZE);
+
+ //Copy message
+ memmove(pBuffer, cCmdDVPtr(ReadDVIndex), DV_ARRAY[ReadDVIndex].Count);
+
+ if (Remove)
+ {
+ //Free memory used by message
+ Status = cCmdFreeDopeVector(ReadDVIndex);
+ if (IS_ERR(Status))
+ return Status;
+
+ SET_READ_DTLG(NOT_A_DS_ID);
+
+ //Advance read index
+ VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
+ }
+ }
+ else
+ {
+ //No message to read, datalog Queue is empty
+ NXT_ASSERT(VarsCmd.DatalogBuffer.ReadIndex == VarsCmd.DatalogBuffer.WriteIndex);
+
+ return (STAT_MSG_EMPTY_MAILBOX);
+ }
+
+ return Status;
+}
+
+
+//
+// Color Sensor Functions
+//
+NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray,
+ SWORD * ScaledArray, UBYTE * InvalidData)
+{
+ ULONG i;
+ //Make sure Port is valid for Color Sensor
+ if (!(pMapInput->Inputs[Port].SensorType == COLORFULL || pMapInput->Inputs[Port].SensorType == COLORRED
+ || pMapInput->Inputs[Port].SensorType == COLORGREEN || pMapInput->Inputs[Port].SensorType == COLORBLUE
+ || pMapInput->Inputs[Port].SensorType == COLORNONE))
+ {
+ return (ERR_COMM_CHAN_NOT_READY); //TODO - is this the right error?
+ }
+ //Copy Detected Color
+ *SensorValue = pMapInput->Inputs[Port].SensorValue;
+
+ //Copy all raw, normalized and scaled data from I/O Map
+ for (i=0; i<NO_OF_COLORS; i++){
+ RawArray[i] = pMapInput->Colors[Port].ADRaw[i];
+ NormalizedArray[i] = pMapInput->Colors[Port].SensorRaw[i];
+ ScaledArray[i] = pMapInput->Colors[Port].SensorValue[i];
+ }
+ //Copy the Invalid Data Flag
+ *InvalidData = pMapInput->Inputs[Port].InvalidData;
+
+ return NO_ERR;
+
+}
+
+
+//
+// Dataspace Support functions
+//
+
+UBYTE cCmdIsDSElementIDSane(DS_ELEMENT_ID Index)
+{
+ if (Index < VarsCmd.DataspaceCount)
+ return TRUE;
+ else
+ return FALSE;
+}
+
+void * cCmdResolveDataArg(DATA_ARG DataArg, UWORD Offset, TYPE_CODE * TypeCode)
+{
+ void * ret_val = NULL;
+
+ //!!! DATA_ARG masking system only for internal c_cmd use!
+ // All normal bytecode arguments should go through top if() block.
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DataArg));
+ ret_val = cCmdDSPtr(DataArg, Offset);
+ if (TypeCode)
+ *TypeCode = VarsCmd.pDataspaceTOC[DataArg].TypeCode;
+
+ //!!! Caller beware! If DataArg isn't sane, ret_val may be out of range or NULL!
+ return ret_val;
+}
+
+// normal Resolve handles both, but this is specific to I/O args
+void * cCmdResolveIODataArg(DATA_ARG DataArg, ULONG Offset, TYPE_CODE * TypeCode)
+ {
+ void * ret_val = NULL;
+
+ ULONG ModuleID;
+ ULONG FieldID;
+ //DataArg refers to a field in the IO map
+ // ModuleID = ((DataArg >> 9) & 0x1F);
+ ModuleID = ((DataArg & 0x3FFF) >> 9);
+ FieldID = (DataArg & 0x01FF);
+
+ //!!! Preliminary bounds check -- still could allow invalid combos through
+ if (ModuleID > MOD_OUTPUT || FieldID >= IO_OUT_FIELD_COUNT)
+ {
+ NXT_BREAK;
+ return NULL;
+ }
+
+ ret_val = IO_PTRS[ModuleID][FieldID];
+ if (TypeCode)
+ *TypeCode = IO_TYPES[ModuleID][FieldID];
+ return ret_val;
+}
+
+void cCmdSetValFlt(void * pVal, TYPE_CODE TypeCode, float NewVal)
+{
+
+ if (pVal)
+ {
+ switch (TypeCode)
+ {
+ case TC_ULONG:
+ case TC_SLONG:
+ {
+ *(ULONG*)pVal = NewVal;
+ }
+ break;
+
+ case TC_UWORD:
+ case TC_SWORD:
+ {
+ *(UWORD*)pVal = (UWORD)NewVal;
+ }
+ break;
+
+ case TC_UBYTE:
+ case TC_SBYTE:
+ {
+ *(UBYTE*)pVal = (UBYTE)NewVal;
+ }
+ break;
+
+ case TC_FLOAT:
+ {
+ *(float*)pVal = (float)NewVal;
+ }
+ break;
+ }
+ }
+
+ return;
+}
+
+ULONG cCmdGetUByte(void * pVal);
+ULONG cCmdGetSByte(void * pVal);
+ULONG cCmdGetUWord(void * pVal);
+ULONG cCmdGetSWord(void * pVal);
+ULONG cCmdGetULong(void * pVal);
+ULONG cCmdGetSLong(void * pVal);
+ULONG cCmdGetError(void * pVal);
+ULONG cCmdGetFloat(void * pVal);
+
+void cCmdSetByte(void * pVal, ULONG NewVal);
+void cCmdSetWord(void * pVal, ULONG NewVal);
+void cCmdSetLong(void * pVal, ULONG NewVal);
+void cCmdSetError(void * pVal, ULONG NewVal);
+
+
+typedef ULONG (*pGetOperand)(void *);
+static pGetOperand GetProcArray[11]= {cCmdGetUByte, cCmdGetUByte, cCmdGetSByte, cCmdGetUWord, cCmdGetSWord, cCmdGetULong, cCmdGetSLong, cCmdGetError, cCmdGetError, cCmdGetError, cCmdGetFloat}; // dup UByte to line up
+
+typedef void (*pSetOperand)(void *, ULONG);
+static pSetOperand SetProcArray[9]= {cCmdSetByte, cCmdSetByte, cCmdSetByte, cCmdSetWord, cCmdSetWord, cCmdSetLong, cCmdSetLong, cCmdSetError, cCmdSetError}; // dup UByte to line up
+
+void cCmdSetError(void * pVal, ULONG NewVal) {
+ NXT_BREAK;
+}
+
+void cCmdSetLong(void * pVal, ULONG NewVal) {
+ *(ULONG*)pVal = NewVal;
+}
+
+void cCmdSetWord(void * pVal, ULONG NewVal) {
+ *(UWORD*)pVal = (UWORD)NewVal;
+}
+
+void cCmdSetByte(void * pVal, ULONG NewVal) {
+ *(UBYTE*)pVal = (UBYTE)NewVal;
+}
+
+// only works on simple types, equivalent to resolve and get, but faster
+ULONG cCmdGetScalarValFromDataArg(DATA_ARG DataArg, UWORD Offset)
+{
+ DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
+ return GetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset + Offset);
+}
+
+
+ULONG cCmdGetError(void * pVal) {
+ NXT_BREAK;
+ return 0;
+}
+
+ULONG cCmdGetULong(void * pVal) {
+ return (ULONG)(*(ULONG*)pVal);
+}
+
+ULONG cCmdGetSLong(void * pVal) {
+ return (SLONG)(*(SLONG*)pVal);
+}
+
+ULONG cCmdGetUWord(void * pVal) {
+ return (UWORD)(*(UWORD*)pVal);
+}
+
+ULONG cCmdGetSWord(void * pVal) {
+ return (SWORD)(*(SWORD*)pVal);
+}
+
+ULONG cCmdGetUByte(void * pVal) {
+ return (UBYTE)(*(UBYTE*)pVal);
+}
+
+ULONG cCmdGetSByte(void * pVal) {
+ return (SBYTE)(*(SBYTE*)pVal);
+}
+
+ULONG cCmdGetFloat(void * pVal) {
+ float tempVal = *(float*)pVal;
+ if (tempVal >= 0) {
+ tempVal += 0.5;
+ }
+ else {
+ tempVal -= 0.5;
+ }
+ return (ULONG)tempVal;
+}
+
+ULONG cCmdGetVal(void * pVal, TYPE_CODE TypeCode)
+{
+ if (pVal)
+ return GetProcArray[TypeCode](pVal);
+ else
+ //!!! Default return value places responsibility on caller to use this function wisely
+ return 0;
+}
+
+
+float cCmdGetValFlt(void * pVal, TYPE_CODE TypeCode)
+{
+ if (pVal)
+ {
+ switch (TypeCode)
+ {
+ case TC_ULONG:
+ {
+ return (ULONG)(*(ULONG*)pVal);
+ }
+
+ case TC_SLONG:
+ {
+ return (SLONG)(*(SLONG*)pVal);
+ }
+
+ case TC_UWORD:
+ {
+ return (UWORD)(*(UWORD*)pVal);
+ }
+
+ case TC_SWORD:
+ {
+ return (SWORD)(*(SWORD*)pVal);
+ }
+
+ case TC_UBYTE:
+ {
+ return (UBYTE)(*(UBYTE*)pVal);
+ }
+
+ case TC_SBYTE:
+ {
+ return (SBYTE)(*(SBYTE*)pVal);
+ }
+
+ case TC_FLOAT:
+ {
+ return (float)(*(float*)pVal);
+ }
+
+ default:
+ break;
+ }
+ }
+
+ //!!! Default return value places responsibility on caller to use this function wisely
+ return 0;
+}
+
+
+
+// Only for scalar types and no offset
+void cCmdSetScalarValFromDataArg(DATA_ARG DataArg, ULONG NewVal)
+{
+ DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
+ SetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset, NewVal);
+}
+
+void cCmdSetVal(void * pVal, TYPE_CODE TypeCode, ULONG NewVal)
+{
+ if (pVal)
+ SetProcArray[TypeCode](pVal, NewVal);
+}
+
+void* cCmdDSPtr(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ void * pDSItem;
+ DV_INDEX DVIndex;
+ TYPE_CODE TypeCode;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+
+ TypeCode = cCmdDSType(DSElementID);
+ if (TypeCode == TC_ARRAY)
+ {
+ //!!! Empty arrays return NULL.
+ if (cCmdArrayCount(DSElementID, Offset) == 0)
+ pDSItem = NULL;
+ else
+ {
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+ pDSItem = (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset);
+ }
+ }
+ else if (TypeCode == TC_CLUSTER)
+ {
+ NXT_ASSERT(cCmdClusterCount(DSElementID) != 0)
+
+ //Returning pointer to the first element in the cluster
+ pDSItem = cCmdDSPtr(INC_ID(DSElementID), Offset);
+ }
+ else
+ pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset);
+
+ NXT_ASSERT((UBYTE*)pDSItem < POOL_SENTINEL);
+
+ return pDSItem;
+}
+
+void* cCmdDVPtr(DV_INDEX DVIndex)
+{
+ NXT_ASSERT(IS_DV_INDEX_SANE(DVIndex));
+ return (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset);
+}
+
+
+//!!! Recursive function
+DS_ELEMENT_ID cCmdNextDSElement(DS_ELEMENT_ID CurrID)
+{
+ DS_ELEMENT_ID NextID;
+ TYPE_CODE CurrType;
+ UWORD ClusterCount, i;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(CurrID));
+
+ NextID = CurrID + 1;
+
+ if (!cCmdIsDSElementIDSane(NextID))
+ return NOT_A_DS_ID;
+
+ CurrType = cCmdDSType(CurrID);
+
+ if (CurrType == TC_ARRAY)
+ {
+ //Arrays contain two elements. Advance past the second one.
+ NextID = cCmdNextDSElement(NextID);
+ }
+ else if (CurrType == TC_CLUSTER)
+ {
+ ClusterCount = cCmdClusterCount(CurrID);
+ for (i = 0; i < ClusterCount; i++)
+ {
+ NextID = cCmdNextDSElement(NextID);
+ }
+ }
+
+ return NextID;
+}
+
+
+//!!! Recursive function
+UBYTE cCmdCompareDSType(DS_ELEMENT_ID DSElementID1, DS_ELEMENT_ID DSElementID2)
+{
+ TYPE_CODE Type1, Type2;
+ UWORD i, Count1, Count2;
+
+ Type1 = cCmdDSType(DSElementID1);
+ Type2 = cCmdDSType(DSElementID2);
+
+ if (Type1 != Type2)
+ return FALSE;
+
+ if (Type1 == TC_CLUSTER)
+ {
+ Count1 = cCmdClusterCount(DSElementID1);
+ Count2 = cCmdClusterCount(DSElementID2);
+
+ if(Count1 != Count2)
+ return FALSE;
+
+ DSElementID1 = INC_ID(DSElementID1);
+ DSElementID2 = INC_ID(DSElementID2);
+
+ for (i = 0; i < Count1; i++)
+ {
+ if (!cCmdCompareDSType(DSElementID1, DSElementID2))
+ return FALSE;
+
+ DSElementID1 = cCmdNextDSElement(DSElementID1);
+ DSElementID2 = cCmdNextDSElement(DSElementID2);
+ }
+ }
+ else if (Type1 == TC_ARRAY)
+ {
+ if (!cCmdCompareDSType(INC_ID(DSElementID1), INC_ID(DSElementID2)))
+ return FALSE;
+ }
+
+ return TRUE;
+}
+
+
+//!!! Recursive function
+UWORD cCmdCalcFlattenedSize(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ UWORD Size = 0;
+ TYPE_CODE TypeCode;
+ DV_INDEX DVIndex;
+ UWORD i;
+ UWORD Count;
+
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (TypeCode == TC_ARRAY)
+ {
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+
+ DSElementID = INC_ID(DSElementID);
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (!IS_AGGREGATE_TYPE(TypeCode))
+ {
+ //Short circuit recursive calculation if our array sub-type is a scalar
+ Size += DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
+ }
+ else
+ {
+ //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
+ for (i = 0; i < DV_ARRAY[DVIndex].Count; i++)
+ {
+ Size += cCmdCalcFlattenedSize(DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
+ }
+ }
+ }
+ else if (TypeCode == TC_CLUSTER)
+ {
+ Count = cCmdClusterCount(DSElementID);
+
+ DSElementID = INC_ID(DSElementID);
+ for (i = 0; i < Count; i++)
+ {
+ Size += cCmdCalcFlattenedSize(DSElementID, Offset);
+ DSElementID = cCmdNextDSElement(DSElementID);
+ }
+ }
+ else //Scalar
+ {
+ Size += cCmdSizeOf(TypeCode);
+ }
+ return Size;
+}
+
+
+//!!! Recursive function
+NXT_STATUS cCmdFlattenToByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode;
+ DV_INDEX DVIndex;
+ UWORD i;
+ UWORD Count;
+ UBYTE *pVal;
+
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (TypeCode == TC_ARRAY)
+ {
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+ Count = DV_ARRAY[DVIndex].Count;
+
+ DSElementID = INC_ID(DSElementID);
+ TypeCode = cCmdDSType(DSElementID);
+ if (!IS_AGGREGATE_TYPE(TypeCode))
+ {
+ //Short circuit recursive calculation if our array sub-type is a scalar
+ Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
+ memmove((pByteArray + *pByteOffset), (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), Count);
+ *pByteOffset += Count;
+ }
+ else
+ {
+ //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
+ for (i = 0; i < Count; i++)
+ {
+ cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
+ }
+ }
+ }
+ else if (TypeCode == TC_CLUSTER)
+ {
+ Count = cCmdClusterCount(DSElementID);
+
+ DSElementID = INC_ID(DSElementID);
+ for (i = 0; i < Count; i++)
+ {
+ cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, Offset);
+ DSElementID = cCmdNextDSElement(DSElementID);
+ }
+ }
+ else //Scalar
+ {
+ pVal = cCmdResolveDataArg(DSElementID, Offset, NULL);
+ Count = cCmdSizeOf(TypeCode);
+
+ memmove((pByteArray + *pByteOffset), pVal, Count);
+ *pByteOffset += Count;
+ }
+
+ return Status;
+}
+
+NXT_STATUS cCmdUnflattenFromByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode;
+ DV_INDEX DVIndex;
+ UWORD i;
+ UWORD Count;
+ UBYTE *pVal;
+
+ TypeCode = cCmdDSType(DSElementID);
+
+ if (TypeCode == TC_ARRAY)
+ {
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+ Count = DV_ARRAY[DVIndex].Count;
+
+ DSElementID = INC_ID(DSElementID);
+ TypeCode = cCmdDSType(DSElementID);
+ if (!IS_AGGREGATE_TYPE(TypeCode))
+ {
+ //Short circuit recursive calculation if our array sub-type is a scalar
+ Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
+ memmove((VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), (pByteArray + *pByteOffset), Count);
+ *pByteOffset += Count;
+ }
+ else
+ {
+ //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
+ for (i = 0; i < Count; i++)
+ {
+ cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
+ }
+ }
+ }
+ else if (TypeCode == TC_CLUSTER)
+ {
+ Count = cCmdClusterCount(DSElementID);
+
+ DSElementID = INC_ID(DSElementID);
+ for (i = 0; i < Count; i++)
+ {
+ cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, Offset);
+ DSElementID = cCmdNextDSElement(DSElementID);
+ }
+ }
+ else //Scalar
+ {
+ pVal = cCmdResolveDataArg(DSElementID, Offset, NULL);
+ Count = cCmdSizeOf(TypeCode);
+
+ memmove(pVal, (pByteArray + *pByteOffset), Count);
+ *pByteOffset += Count;
+ }
+
+ return Status;
+}
+
+
+UWORD cCmdClusterCount(DS_ELEMENT_ID DSElementID)
+{
+ UWORD ClusterCount;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_CLUSTER);
+
+ ClusterCount = VarsCmd.pDataspaceTOC[DSElementID].DSOffset;
+
+ return ClusterCount;
+}
+
+
+UWORD cCmdGetDVIndex(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ UWORD DVIndex;
+
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);
+
+ DVIndex = *(UWORD *)(VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset);
+
+ //Make sure we're returning a valid DVIndex
+ NXT_ASSERT(DVIndex != 0 && DVIndex < DV_ARRAY[0].Count);
+
+ return DVIndex;
+}
+
+
+UWORD cCmdArrayCount(DS_ELEMENT_ID DSElementID, UWORD Offset)
+{
+ DV_INDEX DVIndex;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);
+
+ DVIndex = cCmdGetDVIndex(DSElementID, Offset);
+ return DV_ARRAY[DVIndex].Count;
+}
+
+TYPE_CODE cCmdArrayType(DS_ELEMENT_ID DSElementID)
+{
+ TYPE_CODE TypeCode;
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
+ NXT_ASSERT(cCmdIsDSElementIDSane(INC_ID(DSElementID)));
+ NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);
+
+ TypeCode = VarsCmd.pDataspaceTOC[DSElementID + 1].TypeCode;
+
+ return TypeCode;
+}
+
+
+DS_ELEMENT_ID cCmdGetDataspaceCount(void)
+{
+ return (VarsCmd.DataspaceCount);
+}
+
+
+UBYTE cCmdCompare(UBYTE CompCode, ULONG Val1, ULONG Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
+{
+ SLONG SVal1, SVal2;
+ if (QUICK_UNSIGNED_TEST(TypeCode1) || QUICK_UNSIGNED_TEST(TypeCode2))
+ {
+ return ((CompCode == OPCC1_LT && Val1 < Val2)
+ || (CompCode == OPCC1_GT && Val1 > Val2)
+ || (CompCode == OPCC1_LTEQ && Val1 <= Val2)
+ || (CompCode == OPCC1_GTEQ && Val1 >= Val2)
+ || (CompCode == OPCC1_EQ && Val1 == Val2)
+ || (CompCode == OPCC1_NEQ && Val1 != Val2));
+ }
+ else
+ {
+ SVal1 = (SLONG)Val1;
+ SVal2 = (SLONG)Val2;
+ return ((CompCode == OPCC1_LT && SVal1 < SVal2)
+ || (CompCode == OPCC1_GT && SVal1 > SVal2)
+ || (CompCode == OPCC1_LTEQ && SVal1 <= SVal2)
+ || (CompCode == OPCC1_GTEQ && SVal1 >= SVal2)
+ || (CompCode == OPCC1_EQ && SVal1 == SVal2)
+ || (CompCode == OPCC1_NEQ && SVal1 != SVal2));
+ }
+}
+
+UBYTE cCmdCompareFlt(UBYTE CompCode, float Val1, float Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
+{
+ //!!! add threshold to equality comparisons
+ return ((CompCode == OPCC1_LT && Val1 < Val2)
+ || (CompCode == OPCC1_GT && Val1 > Val2)
+ || (CompCode == OPCC1_LTEQ && Val1 <= Val2)
+ || (CompCode == OPCC1_GTEQ && Val1 >= Val2)
+ || (CompCode == OPCC1_EQ && Val1 == Val2)
+ || (CompCode == OPCC1_NEQ && Val1 != Val2));
+}
+
+
+NXT_STATUS cCmdCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE Finished;
+
+ Finished = FALSE;
+ Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, &Finished, Arg2, Offset2, Arg3, Offset3);
+ if (Finished == FALSE)
+ {
+ //If Finished has not been set to TRUE, it means that it was unable to find an inequality, thereby ending the comparison.
+ //Both elements are equal. Assign the proper value to ReturnBool
+ *ReturnBool = (CompCode == OPCC1_EQ || CompCode == OPCC1_GTEQ || CompCode == OPCC1_LTEQ);
+ }
+
+ return Status;
+}
+
+
+//!!! Recursive function
+NXT_STATUS cCmdRecursiveCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, UBYTE *Finished, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
+{
+ //The value of Finished must be set to FALSE before calling this function.
+ //We are able to determine the result of the comparison once we find an inequality.
+ //Once an inequality is found, Finished is set to TRUE and ReturnBool is set based on the CompCode.
+ //A call to this function will return with Finished still equal to FALSE if both elements are equal in value and count.
+ //It is the caller of this function's job to set ReturnBool if this function returns with Finished == FALSE.
+
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode2, TypeCode3;
+ DV_INDEX DVIndex2, DVIndex3;
+ ULONG ArgVal2, ArgVal3;
+ UWORD Count2, Count3, MinCount;
+ UWORD i;
+
+ TypeCode2 = cCmdDSType(Arg2);
+ TypeCode3 = cCmdDSType(Arg3);
+
+ //Make sure the two things we're comparing are the same type
+ if (IS_AGGREGATE_TYPE(TypeCode2) && (TypeCode2 != TypeCode3))
+ {
+ NXT_BREAK;
+ return ERR_ARG;
+ }
+
+ //Simple case, both args are scalars. Solve and return.
+ if (!IS_AGGREGATE_TYPE(TypeCode2))
+ {
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
+ ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);
+
+ //Once we find an inequality, we can determine the result of the comparison
+ *Finished = cCmdCompare(OPCC1_NEQ, ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+
+ if (*Finished)
+ *ReturnBool = cCmdCompare(CompCode, ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+
+ return Status;
+ }
+
+ // Initialize local variables for each argument
+
+ if (TypeCode2 == TC_ARRAY)
+ {
+ Count2 = cCmdArrayCount(Arg2, Offset2);
+ DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
+ Offset2 = DV_ARRAY[DVIndex2].Offset;
+
+ Count3 = cCmdArrayCount(Arg3, Offset3);
+ DVIndex3 = cCmdGetDVIndex(Arg3, Offset3);
+ Offset3 = DV_ARRAY[DVIndex3].Offset;
+ }
+ else if (TypeCode2 == TC_CLUSTER)
+ {
+ Count2 = cCmdClusterCount(Arg2);
+ Count3 = cCmdClusterCount(Arg3);
+ }
+
+ //Short circuit evaluation of EQ and NEQ if counts are different
+ if (Count2 != Count3)
+ {
+ if ((CompCode == OPCC1_EQ) || (CompCode == OPCC1_NEQ))
+ {
+ *Finished = TRUE;
+ *ReturnBool = (CompCode == OPCC1_NEQ);
+ return Status;
+ }
+ }
+
+ MinCount = (Count2 < Count3) ? Count2 : Count3;
+
+ //Advance aggregate args to first sub-element for next call
+ Arg2 = INC_ID(Arg2);
+ Arg3 = INC_ID(Arg3);
+
+ //
+ // Loop through the sub-elements of aggregate arguments.
+ // Call cCmdRecursiveCompareAggregates recursively with simpler type.
+ //
+
+ for (i = 0; i < MinCount; i++)
+ {
+ Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, Finished, Arg2, Offset2, Arg3, Offset3);
+ if (*Finished || IS_ERR(Status))
+ return Status;
+
+ //Advance aggregate args to next sub-element
+ if (TypeCode2 == TC_ARRAY)
+ {
+ Offset2 += DV_ARRAY[DVIndex2].ElemSize;
+ Offset3 += DV_ARRAY[DVIndex3].ElemSize;
+ }
+ else if (TypeCode2 == TC_CLUSTER)
+ {
+ Arg2 = cCmdNextDSElement(Arg2);
+ Arg3 = cCmdNextDSElement(Arg3);
+ }
+ }
+
+ //All elements in aggregates type up to MinCount are equal. Count discrepancy determines comparison outcome.
+ if (Count2 != Count3)
+ {
+ *Finished = TRUE;
+ *ReturnBool = cCmdCompare(CompCode, Count2, Count3, TC_UWORD, TC_UWORD);
+ }
+ //Else, no size discrepancy. Elements are equal. Comparison still not resolved,
+ //so return !Finished and status back up the call chain for further comparison
+
+ return Status;
+}
+
+ULONG gClearProfileInfo= 0, bigExecTime= 0;
+#if VMProfilingCode
+void UpdateProfileInfo(ULONG shortOp, CODE_WORD *pInstr, ULONG execTime, ULONG InstrSize)
+{
+ ULONG j;
+ ULONG opCode;
+
+ if(execTime > 500 && shortOp == 8)
+ bigExecTime= shortOp;
+ if(gClearProfileInfo) {
+ ExecutedInstrs= 0;
+ CmdCtrlTime= 0;
+ OverheadTime= 0;
+ CmdCtrlCalls= 0;
+ LastAvgCount= 0;
+ for(j= 0; j < 255; j++)
+ CmdCtrlClumpTime[j]= 0;
+ for(j= 0; j < OPCODE_COUNT; j++) {
+ InstrProfile[j].Avg= 0;
+ InstrProfile[j].Time= 0;
+ InstrProfile[j].Count= 0;
+ InstrProfile[j].Max= 0;
+ }
+ for(j= 0; j < SYSCALL_COUNT; j++) {
+ SysCallProfile[j].Avg= 0;
+ SysCallProfile[j].Time= 0;
+ SysCallProfile[j].Count= 0;
+ SysCallProfile[j].Max= 0;
+ }
+ for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++) {
+ ShortInstrProfile[j].Avg= 0;
+ ShortInstrProfile[j].Time= 0;
+ ShortInstrProfile[j].Count= 0;
+ ShortInstrProfile[j].Max= 0;
+ }
+ for(j= 0; j < NUM_INTERP_FUNCS; j++) {
+ InterpFuncProfile[j].Avg= 0;
+ InterpFuncProfile[j].Time= 0;
+ InterpFuncProfile[j].Count= 0;
+ InterpFuncProfile[j].Max= 0;
+ }
+ gClearProfileInfo= FALSE;
+ }
+ ExecutedInstrs ++;
+ if(shortOp > 7) // shortop bit set
+ {
+ ShortInstrProfile[shortOp-8].Time += execTime;
+ ShortInstrProfile[shortOp-8].Count++;
+ if(execTime > ShortInstrProfile[shortOp-8].Max)
+ ShortInstrProfile[shortOp-8].Max= execTime;
+ }
+ else
+ {
+ opCode = OP_CODE(pInstr);
+ InstrProfile[opCode].Time += execTime;
+ InstrProfile[opCode].Count++;
+ if(execTime > InstrProfile[opCode].Max)
+ InstrProfile[opCode].Max= execTime;
+ if(opCode == OP_SYSCALL)
+ {
+ SysCallProfile[GetDataArg(pInstr[1])].Time += execTime;
+ SysCallProfile[GetDataArg(pInstr[1])].Count++;
+ if(execTime > SysCallProfile[GetDataArg(pInstr[1])].Max)
+ SysCallProfile[GetDataArg(pInstr[1])].Max= execTime;
+ }
+
+ InterpFuncProfile[InstrSize].Time += execTime;
+ InterpFuncProfile[InstrSize].Count++;
+ if(execTime > InterpFuncProfile[InstrSize].Max)
+ InterpFuncProfile[InstrSize].Max= execTime;
+ }
+ if(ExecutedInstrs - LastAvgCount > 999) // every N instrs, update avgs
+ {
+ for(j= 0; j < OPCODE_COUNT; j++)
+ if(InstrProfile[j].Count)
+ InstrProfile[j].Avg= InstrProfile[j].Time/InstrProfile[j].Count;
+ for(j= 0; j < SYSCALL_COUNT; j++)
+ if(SysCallProfile[j].Count)
+ SysCallProfile[j].Avg= SysCallProfile[j].Time/SysCallProfile[j].Count;
+ for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++)
+ if(ShortInstrProfile[j].Count)
+ ShortInstrProfile[j].Avg= ShortInstrProfile[j].Time/ShortInstrProfile[j].Count;
+ for(j= 0; j < NUM_INTERP_FUNCS; j++)
+ if(InterpFuncProfile[j].Count)
+ InterpFuncProfile[j].Avg= InterpFuncProfile[j].Time/InterpFuncProfile[j].Count;
+ LastAvgCount= ExecutedInstrs;
+ }
+}
+#endif
+
+
+//
+// Interpreter Functions
+//
+
+NXT_STATUS cCmdInterpFromClump()
+{
+ CLUMP_ID Clump= VarsCmd.RunQ.Head;
+ NXT_STATUS Status = NO_ERR;
+ CLUMP_REC * pClumpRec;
+ CODE_WORD * pInstr, *lastClumpInstr;
+ UBYTE InstrSize;
+ ULONG shortOp, nextMSTick;
+ SLONG i;
+#if VM_BENCHMARK
+ ULONG InstrTime = dTimerRead();
+#endif
+
+ if (!cCmdIsClumpIDSane(Clump)) // this means all clumps are asleep
+ return TIMES_UP;
+
+ //Resolve clump record structure and current instruction pointer
+ pClumpRec = &(VarsCmd.pAllClumps[Clump]);
+ pInstr = pClumpRec->PC; // abs
+ lastClumpInstr= pClumpRec->CodeEnd; // abs
+
+ i= gInstrsToExecute;
+ nextMSTick= dTimerGetNextMSTickCnt();
+ do
+ {
+#if VMProfilingCode
+ ULONG instrStartTime;
+ instrStartTime= dTimerReadHiRes();
+#endif
+
+ ULONG instrWord= *(UWORD*)pInstr;
+ shortOp= (instrWord>>8) & 0x0F;
+ if(shortOp > 7) // shortop bit set
+ Status= ShortInterpFuncs[shortOp - 8](pInstr);
+ else
+ { // we know this is a long instr, dispatch on num params, which correlates to size
+ InstrSize = INSTR_SIZE(instrWord); // keep in a local for profiling
+ Status = (*InterpFuncs[InstrSize])(pInstr);
+ }
+
+#if VMProfilingCode
+ UpdateProfileInfo(shortOp, pInstr, dTimerReadHiRes() - instrStartTime, InstrSize);
+#endif
+
+afterCompaction:
+ if (Status == NO_ERR)
+ pInstr += gPCDelta;
+ else if (Status == CLUMP_DONE) // already requeued
+ {
+ pClumpRec->PC = pClumpRec->CodeStart;
+ pClumpRec->CurrFireCount = pClumpRec->InitFireCount;
+ return Status;
+ }
+ else if (Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == ROTATE_QUEUE) // already requeued
+ {
+ pClumpRec->PC = pInstr + gPCDelta;
+ //Throw error if we ever advance beyond the clump's codespace
+ if (pInstr > lastClumpInstr)
+ {
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+ return Status;
+ }
+ else if (Status == ERR_MEM)
+ {
+ //Memory is full. Compact dataspace and try the instruction again.
+ //!!! Could compact DopeVectorArray here
+ cCmdDSCompact();
+ if(shortOp > 7) // shortop bit set
+ Status= ShortInterpFuncs[shortOp - 8](pInstr);
+ else
+ Status = (*InterpFuncs[InstrSize])(pInstr);
+ if(Status == ERR_MEM)
+ return Status;
+ else
+ goto afterCompaction;
+ }
+ else // other errors, breakout, stop
+ return Status;
+
+ //Throw error if we ever advance beyond the clump's codespace
+ if (pInstr > lastClumpInstr)
+ {
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+
+#if VM_BENCHMARK
+ //Increment opcode count
+ VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][0]++;
+
+ InstrTime = dTimerRead() - InstrTime;
+ if (InstrTime > 1)
+ {
+ VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][1]++;
+ if (InstrTime > VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2])
+ VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2] = InstrTime;
+ }
+ VarsCmd.InstrCount++;
+#endif
+
+ //Count one more instruction for this pass
+ if ((SLONG)(nextMSTick - dTimerReadTicks()) <= 0) // HWTimer has passed ms tick limit
+ Status= TIMES_UP;
+ else if(--i <= 0)
+ Status= ROTATE_QUEUE;
+ } while (!Status);
+ pClumpRec->PC= pInstr;
+ return (Status);
+}
+
+
+NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE opCode;
+ DATA_ARG Arg1;
+
+ NXT_ASSERT(pCode != NULL);
+
+ gPCDelta= 2;
+ opCode = OP_CODE(pCode);
+ Arg1 = pCode[1];
+
+ switch (opCode)
+ {
+ case OP_JMP:
+ {
+ gPCDelta= (SWORD)Arg1;
+ Status = NO_ERR;
+ }
+ break;
+
+ case OP_ACQUIRE:
+ {
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
+ NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);
+
+ Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
+ }
+ break;
+
+ case OP_RELEASE:
+ {
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
+ NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);
+
+ Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
+ }
+ break;
+
+ case OP_SUBRET:
+ {
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
+
+ //Take Subroutine off RunQ
+ //Add Subroutine's caller to RunQ
+ cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head);
+ cCmdEnQClump(&(VarsCmd.RunQ), *((CLUMP_ID *)cCmdDSScalarPtr(Arg1, 0)));
+
+ Status = CLUMP_DONE;
+ }
+ break;
+
+ case OP_FINCLUMPIMMED:
+ {
+ CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
+ cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
+ cCmdSchedDependent(Clump, (CLUMP_ID)Arg1); // Use immediate form
+
+ Status = CLUMP_DONE;
+ }
+ break;
+
+ case OP_GETTICK:
+ {
+ cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
+ }
+ break;
+
+ case OP_STOP:
+ {
+ //Unwired Arg1 means always stop
+ if (Arg1 == NOT_A_DS_ID)
+ Status = STOP_REQ;
+ else if (cCmdGetScalarValFromDataArg(Arg1, 0) > 0)
+ Status = STOP_REQ;
+ }
+ break;
+
+ default:
+ {
+ //Fatal error: Unrecognized instruction
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+ break;
+ }
+
+ return (Status);
+}
+
+ULONG scalarNots= 0, scalarBrtst= 0, scalarUn2Other= 0, scalarUn2Dispatch= 0, polyUn2Dispatch= 0;
+NXT_STATUS cCmdInterpScalarUnop2(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ UBYTE opCode;
+
+ NXT_ASSERT(pCode != NULL);
+ opCode = OP_CODE(pCode);
+ DATA_ARG Arg1, Arg2;
+
+ scalarUn2Dispatch ++;
+ if(opCode == OP_NOT) // t2 && t3 guaranteed scalar
+ {
+ gPCDelta= 3;
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ ULONG ArgVal1, ArgVal2;
+
+ ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
+ //!!! OP_NOT is logical, *not* bit-wise.
+ //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
+ ArgVal1= (!ArgVal2);
+ cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
+ Status = NO_ERR;
+ scalarNots ++;
+ }
+ else if(opCode == OP_BRTST)
+ {
+ ULONG Branch, compare= COMP_CODE(pCode);
+ ULONG TypeCode;
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ TypeCode = cCmdDSType(Arg2);
+
+ if(Arg2 == NOT_A_DS_ID)
+ {
+ Branch= ((compare == OPCC1_EQ)
+ || (compare == OPCC1_LTEQ)
+ || (compare == OPCC1_GTEQ));
+ }
+ else
+ {
+ if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
+ {
+ UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
+ Branch= *pBRVal == 0;
+ }
+ else
+ {
+ SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
+ Branch= ((compare == OPCC1_EQ && SVal1 == 0)
+ || (compare == OPCC1_NEQ && SVal1 != 0)
+ || (compare == OPCC1_GT && SVal1 > 0)
+ || (compare == OPCC1_LT && SVal1 < 0)
+ || (compare == OPCC1_LTEQ && SVal1 <= 0)
+ || (compare == OPCC1_GTEQ && SVal1 >= 0));
+ }
+ }
+ if (Branch)
+ gPCDelta = (SWORD)Arg1;
+ else
+ gPCDelta= 3;
+ Status = NO_ERR;
+ scalarBrtst ++;
+ }
+ else {
+ Status= cCmdInterpUnop2(pCode);
+ scalarUn2Other ++;
+ }
+ return Status;
+}
+
+NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE opCode;
+ DATA_ARG Arg1;
+ DATA_ARG Arg2;
+ void *pArg1 = NULL, *pArg2 = NULL;
+ TYPE_CODE TypeCode1, TypeCode2;
+
+ ULONG i;
+ UWORD ArgC;
+ static UBYTE * ArgV[MAX_CALL_ARGS + 1];
+
+ polyUn2Dispatch ++;
+ UWORD Count;
+ UWORD Offset;
+ SLONG TmpSLong;
+ ULONG TmpULong;
+ ULONG ArgVal2;
+ float FltArgVal2;
+ char Buffer[30];
+ char FormatString[5];
+ UBYTE CheckTrailingZeros = 0;
+
+ NXT_ASSERT(pCode != NULL);
+
+ gPCDelta= 3;
+ opCode = OP_CODE(pCode);
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+
+ if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST || opCode == OP_SQRT || opCode == OP_ABS)
+ {
+ return cCmdInterpPolyUnop2(*pCode, Arg1, 0, Arg2, 0);
+ }
+
+ switch (opCode)
+ {
+ case OP_MOV:
+ {
+ Status= cCmdMove(Arg1, Arg2);
+ }
+ break;
+
+ case OP_SET:
+ {
+ //!!! Should throw error if TypeCode1 is non-scalar
+ // Accepting non-scalar destinations could have unpredictable results!
+ cCmdSetScalarValFromDataArg(Arg1, Arg2);
+ }
+ break;
+
+ case OP_BRTST:
+ {
+ //!!!BDUGGAN BRTST w/ Float?
+ ULONG Branch, compare= COMP_CODE(pCode);
+ ULONG TypeCode = cCmdDSType(Arg2);
+ if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
+ {
+ UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
+ Branch= *pBRVal == 0;
+ }
+ else
+ {
+ SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
+ Branch= ((compare == OPCC1_EQ && SVal1 == 0)
+ || (compare == OPCC1_NEQ && SVal1 != 0)
+ || (compare == OPCC1_GT && SVal1 > 0)
+ || (compare == OPCC1_LT && SVal1 < 0)
+ || (compare == OPCC1_LTEQ && SVal1 <= 0)
+ || (compare == OPCC1_GTEQ && SVal1 >= 0));
+ }
+ if (Branch)
+
+ {
+ gPCDelta = (SWORD)Arg1;
+ Status = NO_ERR;
+ }
+ }
+ break;
+
+ case OP_FINCLUMP:
+ {
+ CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
+ cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
+ cCmdSchedDependents(Clump, (SWORD)Arg1, (SWORD)Arg2);
+ Status = CLUMP_DONE;
+ }
+ break;
+
+ case OP_SUBCALL:
+ {
+ NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1));
+ NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1));
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg2));
+
+ *((CLUMP_ID *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;
+
+ cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
+ cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1); //Add callee to RunQ
+
+ Status = CLUMP_SUSPEND;
+ }
+ break;
+
+ case OP_ARRSIZE:
+ {
+ cCmdSetScalarValFromDataArg(Arg1, cCmdArrayCount(Arg2, 0));
+ }
+ break;
+
+ case OP_SYSCALL:
+ {
+ if (Arg1 >= SYSCALL_COUNT)
+ {
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ break;
+ }
+
+ ArgC = cCmdClusterCount(Arg2);
+
+ if (ArgC > MAX_CALL_ARGS)
+ {
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ break;
+ }
+
+ if (ArgC > 0)
+ {
+ Arg2 = INC_ID(Arg2);
+
+ for (i = 0; i < ArgC; i++)
+ {
+ if (cCmdDSType(Arg2) == TC_ARRAY)
+ {
+ //Storing pointer to array's DV_INDEX
+ //!!! This resolve is different than cCmdDSPtr
+ // since SysCalls may need the DVIndex to re-alloc arrays
+ ArgV[i] = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset;
+ }
+ else
+ {
+ ArgV[i] = cCmdDSPtr(Arg2, 0);
+ }
+
+ //If any argument fails to resolve, return a fatal error.
+ if (ArgV[i] == NULL)
+ {
+ Status = ERR_BAD_PTR;
+ break;
+ }
+
+ Arg2 = cCmdNextDSElement(Arg2);
+ }
+ }
+ else
+ {
+ i = 0;
+ }
+
+ //ArgV list is null terminated
+ ArgV[i] = NULL;
+
+ Status = (*SysCallFuncs[Arg1])(ArgV);
+ }
+ break;
+
+ case OP_FLATTEN:
+ {
+ //Flatten Arg2 to a NULL terminated string
+
+ //Assert that the destination is a string (array of bytes)
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);
+
+ Count = cCmdCalcFlattenedSize(Arg2, 0);
+ //Add room for NULL terminator
+ Count++;
+ Status = cCmdDSArrayAlloc(Arg1, 0, Count);
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ Offset = 0;
+
+ Status = cCmdFlattenToByteArray(pArg1, &Offset, Arg2, 0);
+ //Append NULL terminator
+ *((UBYTE *)pArg1 + Offset) = 0;
+ Offset++;
+ NXT_ASSERT(Offset == Count);
+ }
+ break;
+
+ case OP_NUMTOSTRING:
+ {
+ //Assert that the destination is a string (array of bytes)
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);
+
+ //Make sure we're trying to convert a scalar to a string
+ TypeCode2= cCmdDSType(Arg2);
+ NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode2));
+
+ if (TypeCode2 == TC_FLOAT)
+ {
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+ FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
+ // is number too big for display? then format differently and don't bother with trailing zeros
+ if ((FltArgVal2 > 9999999999999.99)||(FltArgVal2 < -999999999999.99)){ // these are the widest %.2f numbers that will fit on display
+ strcpy (FormatString, "%.6g");
+ }
+ else{
+ strcpy (FormatString, "%.2f");
+ CheckTrailingZeros = 1;
+ }
+ Count = sprintf(Buffer, FormatString, FltArgVal2);
+ Count++; //add room for null terminator
+
+ if (CheckTrailingZeros){
+ // Determine if the trailing digits are zeros. If so, drop them
+ if (Buffer[Count-2] == 0x30) { // NOTE: 0x30 is ASCII 0
+ if (Buffer[Count-3] == 0x30){
+ strcpy (FormatString, "%.0f"); // the last two digits = 0, copy as integer
+ Count = Count - 3; // don't need memory for decimal and 2 ascii characters
+ }
+ else {
+ strcpy (FormatString, "%.1f"); // only the 2nd digit = 0 so drop it, but keep the tenths place
+ Count = Count - 1; // don't need memory for 2nd ascii character
+ }
+ }
+ }
+ }
+ else
+ {
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
+ //Calculate size of array
+ if (ArgVal2 == 0)
+ Count = 1;
+ else {
+ Count = 0;
+ SLONG digits= 0;
+ ULONG Tmp= 1;
+ if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE)
+ {
+ TmpSLong = (SLONG)ArgVal2;
+ //Add room for negative sign
+ if (TmpSLong < 0) {
+ Count++;
+ TmpULong= -TmpSLong;
+ }
+ else
+ TmpULong= ArgVal2;
+ }
+ else
+ TmpULong= ArgVal2;
+
+ while (Tmp <= TmpULong && digits < 10) { // maxint is ten digits, max
+ Tmp *= 10;
+ digits++;
+ }
+ Count += digits;
+ }
+ //add room for NULL terminator
+ Count++;
+ }
+
+ //Allocate array
+ Status = cCmdDSArrayAlloc(Arg1, 0, Count);
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1);
+
+ //Populate array
+ if (TypeCode2 == TC_FLOAT)
+ {
+ sprintf(pArg1, FormatString, FltArgVal2);
+ }
+ else if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE)
+ {
+ sprintf(pArg1, "%d", (SLONG)ArgVal2);
+ }
+ else
+ {
+ sprintf(pArg1, "%u", ArgVal2);
+ }
+ }
+ break;
+
+ case OP_STRTOBYTEARR:
+ {
+ NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE));
+ NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE));
+
+ Count = cCmdArrayCount(Arg2, 0);
+
+ if (Count > 0)
+ {
+ Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count - 1));
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+
+ memmove(pArg1, pArg2, Count - 1);
+ }
+ }
+ break;
+
+ case OP_BYTEARRTOSTR:
+ {
+ NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE));
+ NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE));
+
+ Count = cCmdArrayCount(Arg2, 0);
+
+ Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count + 1));
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+
+ memmove(pArg1, pArg2, Count);
+ *((UBYTE *)pArg1 + Count) = '\0';
+ }
+ break;
+
+ case OP_WAIT:
+ {
+ ULONG wait= 0;
+ //Unwired Arg2 defaults to wait 0, which rotates queue
+ if (Arg2 != NOT_A_DS_ID)
+ wait= cCmdGetScalarValFromDataArg(Arg2, 0);
+ if(wait == 0)
+ Status= ROTATE_QUEUE;
+ else
+ Status = cCmdSleepClump(wait + IOMapCmd.Tick); // put to sleep, to wake up wait ms in future
+ if(Arg1 != NOT_A_DS_ID)
+ cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
+ }
+ break;
+
+ default:
+ {
+ //Fatal error: Unrecognized instruction
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+ break;
+ }
+
+ return (Status);
+}
+
+
+NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1Param, DATA_ARG Arg2, UWORD Offset2Param)
+{
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode1, TypeCode2;
+ DV_INDEX DVIndex1, DVIndex2;
+ ULONG ArgVal1, ArgVal2;
+ float FltArgVal1, FltArgVal2;
+ UWORD Count1, Count2, Offset1= Offset1Param, Offset2= Offset2Param;
+ UWORD MinArrayCount;
+ UWORD i;
+ //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters.
+ // In practice, the logic it uses is broken, leading to some source cluster elements being ignored.
+ UBYTE AdvCluster;
+
+ void * pArg1 = NULL,
+ *pArg2 = NULL;
+
+ TypeCode1 = cCmdDSType(Arg1);
+ TypeCode2 = cCmdDSType(Arg2);
+
+ //Simple case, scalar. Solve and return.
+ if (!IS_AGGREGATE_TYPE(TypeCode2))
+ {
+ NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1));
+
+ pArg1 = cCmdResolveDataArg(Arg1, Offset1, &TypeCode1);
+
+ if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT)
+ {
+ pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2);
+ FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
+ FltArgVal1 = cCmdUnop2Flt(Code, FltArgVal2, TypeCode2);
+ cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
+ }
+ else
+ {
+ ArgVal2= cCmdGetScalarValFromDataArg(Arg2, Offset2);
+ if(OP_CODE(&Code) == OP_MOV)
+ ArgVal1= ArgVal2;
+ else
+ ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2);
+ cCmdSetVal(pArg1, TypeCode1, ArgVal1);
+ }
+ return Status;
+ }
+
+ //At least one of the args is an aggregate type
+
+ if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY) {
+ TYPE_CODE tc1, tc2;
+ tc1= cCmdDSType(INC_ID(Arg1));
+ tc2= cCmdDSType(INC_ID(Arg2));
+ if(tc1 <= TC_LAST_INT_SCALAR && tc1 == tc2) {
+ void *pArg1, *pArg2;
+ ULONG Count = cCmdArrayCount(Arg2, Offset2);
+ Status = cCmdDSArrayAlloc(Arg1, Offset1, Count);
+ if (IS_ERR(Status))
+ return Status;
+ pArg1 = cCmdResolveDataArg(Arg1, Offset1, NULL);
+ pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
+ memmove(pArg1, pArg2, Count * cCmdSizeOf(tc1));
+ return Status;
+ }
+ }
+
+
+ //
+ // Initialize Count and ArrayType local variables for each argument
+ //
+
+ if (TypeCode2 == TC_ARRAY)
+ {
+ Count2 = cCmdArrayCount(Arg2, Offset2);
+ DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
+ Offset2 = DV_ARRAY[DVIndex2].Offset;
+ }
+ else if (TypeCode2 == TC_CLUSTER)
+ {
+ Count2 = cCmdClusterCount(Arg2);
+ }
+
+ if (TypeCode1 == TC_ARRAY)
+ {
+ if (TypeCode2 != TC_ARRAY)
+ {
+ //If output is an array, but source is not an array, that's a fatal error!
+ NXT_BREAK;
+ return (ERR_ARG);
+ }
+ if(Count2 == 0) { // both arrays, input is empty, is output already empty?
+ Count1= cCmdArrayCount(Arg1, Offset1);
+ if(Count1 == 0)
+ return NO_ERR;
+ }
+
+ MinArrayCount = Count2;
+
+ //Make sure the destination array is the proper size to hold the result
+ Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount);
+ if (IS_ERR(Status))
+ return Status;
+
+ if(MinArrayCount == 0) // if we emptied array, nothing else to do.
+ return NO_ERR;
+ Count1 = MinArrayCount;
+ DVIndex1 = cCmdGetDVIndex(Arg1, Offset1);
+ Offset1 = DV_ARRAY[DVIndex1].Offset;
+ AdvCluster = FALSE;
+ }
+ else if (TypeCode1 == TC_CLUSTER)
+ {
+ Count1 = cCmdClusterCount(Arg1);
+ AdvCluster = TRUE;
+ }
+
+ //Advance aggregate args to first sub-element for next call
+ if (IS_AGGREGATE_TYPE(TypeCode1))
+ Arg1 = INC_ID(Arg1);
+ if (IS_AGGREGATE_TYPE(TypeCode2))
+ Arg2 = INC_ID(Arg2);
+
+ //
+ // Loop through the sub-elements of aggregate arguments.
+ // Call cCmdInterpPolyUnop2 recursively with simpler type.
+ //
+ for (i = 0; i < Count1; i++)
+ {
+ Status = cCmdInterpPolyUnop2(Code, Arg1, Offset1, Arg2, Offset2);
+ if (IS_ERR(Status))
+ return Status;
+
+ //Advance aggregate args to next sub-element
+ if (TypeCode1 == TC_ARRAY)
+ Offset1 += DV_ARRAY[DVIndex1].ElemSize;
+ else if ((TypeCode1 == TC_CLUSTER) && AdvCluster)
+ Arg1 = cCmdNextDSElement(Arg1);
+
+ if (TypeCode2 == TC_ARRAY)
+ Offset2 += DV_ARRAY[DVIndex2].ElemSize;
+ else if ((TypeCode2 == TC_CLUSTER) && AdvCluster)
+ Arg2 = cCmdNextDSElement(Arg2);
+ }
+ return Status;
+}
+
+
+ULONG cCmdUnop2(CODE_WORD const Code, ULONG Operand, TYPE_CODE TypeCode)
+{
+ UBYTE opCode;
+
+ opCode = OP_CODE((&Code));
+ if(opCode == OP_MOV)
+ return Operand;
+ else if(opCode == OP_NEG)
+ return (-((SLONG)Operand));
+ else if(opCode == OP_NOT)
+ //!!! OP_NOT is logical, *not* bit-wise.
+ //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
+ return (!Operand);
+ else if(opCode == OP_TST)
+ return cCmdCompare(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
+ else if(opCode == OP_ABS)
+ return abs(Operand);
+ else
+ {
+ //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
+ NXT_BREAK;
+ return 0;
+ }
+}
+
+float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode)
+{
+ UBYTE opCode;
+
+ opCode = OP_CODE((&Code));
+ if(opCode == OP_MOV)
+ return Operand;
+ else if(opCode == OP_NEG)
+ return (-(Operand));
+ else if(opCode == OP_NOT)
+ //!!! OP_NOT is logical, *not* bit-wise.
+ //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
+ return (!Operand);
+ else if(opCode == OP_TST)
+ return cCmdCompareFlt(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
+ else if(opCode == OP_ABS)
+ return fabsf(Operand);
+ else if(opCode == OP_SQRT)
+ return sqrt(Operand);
+#if 0
+ else if(opCode == OP_SIN)
+ return sin(Operand);
+ else if(opCode == OP_COS)
+ return cos(Operand);
+ else if(opCode == OP_TAN)
+ return tan(Operand);
+ else if(opCode == OP_ASIN)
+ return asin(Operand);
+ else if(opCode == OP_ACOS)
+ return acos(Operand);
+ else if(opCode == OP_ATAN)
+ return atan(Operand);
+#endif
+ else
+ {
+ //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
+ NXT_BREAK;
+ return 0;
+ }
+}
+
+NXT_STATUS cCmdIOGetSet(ULONG opCode, DATA_ARG Arg1, DATA_ARG Arg2, DATA_ARG Arg3)
+{
+ ULONG ArgVal1, ArgVal2;
+ TYPE_CODE TypeCode2;
+ void *pArg2 = NULL;
+ switch(opCode)
+ {
+ case OP_GETOUT:
+ {
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
+ Arg2 = (UWORD)(0x200 | (Arg3 + ArgVal2 * IO_OUT_FPP));
+ pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
+ cCmdSetScalarValFromDataArg(Arg1, cCmdGetVal(pArg2, TypeCode2));
+ }
+ break;
+ //!!! All IO map access commands should screen illegal port values!
+ // Right now, cCmdResolveIODataArg's implementation allows SETIN/GETIN to access arbitrary RAM!
+ case OP_SETIN:
+ {
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
+ Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
+ pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
+ ArgVal1 = cCmdGetScalarValFromDataArg(Arg1, 0);
+ cCmdSetVal(pArg2, TypeCode2, ArgVal1);
+ }
+ break;
+ case OP_GETIN:
+ {
+ TYPE_CODE TypeCode1;
+ void * pArg1;
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
+ Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
+ pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
+ TypeCode1= cCmdDSType(Arg1);
+ pArg1= cCmdDSScalarPtr(Arg1, 0);
+ if(TypeCode1 <= TC_SBYTE && TypeCode1 <= TC_SBYTE) // seems really common
+ *(UBYTE*)pArg1= *(UBYTE*)pArg2;
+ else
+ cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2));
+ }
+ break;
+ }
+ return NO_ERR;
+}
+
+ULONG scalarCmp= 0, scalarFloatCmp= 0, recursiveCmp= 0, PolyScalarCmp= 0, polyPolyCmp= 0, scalarOther= 0, scalarBinopDispatch= 0, polyBinopDispatch= 0;
+NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ UBYTE opCode;
+ UBYTE CmpBool;
+
+ NXT_ASSERT(pCode != NULL);
+ opCode = OP_CODE(pCode);
+ DATA_ARG Arg1, Arg2, Arg3;
+
+ scalarBinopDispatch ++;
+ if(opCode == OP_CMP) // t2 && t3 guaranteed scalar or string
+ {
+ gPCDelta= 4;
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ ULONG ArgVal1, ArgVal2, ArgVal3;
+ TYPE_CODE TypeCode2, TypeCode3;
+ DS_TOC_ENTRY *dsTOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
+ DS_TOC_ENTRY *dsTOC3Ptr= &VarsCmd.pDataspaceTOC[Arg3];
+
+ TypeCode2 = dsTOC2Ptr->TypeCode;
+ TypeCode3 = dsTOC3Ptr->TypeCode;
+ if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
+ ArgVal2= GetProcArray[TypeCode2](VarsCmd.pDataspace + dsTOC2Ptr->DSOffset);
+ ArgVal3= GetProcArray[TypeCode3](VarsCmd.pDataspace + dsTOC3Ptr->DSOffset);
+ ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+ DS_TOC_ENTRY *dsTOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1];
+ SetProcArray[dsTOC1Ptr->TypeCode](VarsCmd.pDataspace + dsTOC1Ptr->DSOffset, ArgVal1);
+ scalarCmp++;
+ Status = NO_ERR;
+ }
+ else if (TypeCode2 == TC_ARRAY) // two strings
+ {
+ // memcmp(); here or in compareagg, could use memcmp to speed up string compares ???
+ Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
+ cCmdSetScalarValFromDataArg(Arg1, CmpBool);
+ recursiveCmp++;
+ }
+ else { // floats
+ Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
+ scalarFloatCmp++;
+ }
+ }
+ else if(opCode == OP_BRCMP) { // t2 and t3 guaranteed scalar
+ TYPE_CODE TypeCode2, TypeCode3;
+ ULONG ArgVal2, ArgVal3;
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ TypeCode2= cCmdDSType(Arg2);
+ TypeCode3= cCmdDSType(Arg3);
+ ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
+ ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
+ CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+
+ if (CmpBool)
+ gPCDelta = (SWORD)Arg1;
+ else
+ gPCDelta= 4;
+ Status= NO_ERR;
+ }
+ else if(opCode >= OP_SETIN && opCode <= OP_GETOUT) {
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
+ gPCDelta= 4;
+ }
+ else {
+ scalarOther ++;
+ Status= cCmdInterpBinop(pCode);
+ }
+ return Status;
+}
+
+
+NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE opCode;
+ DATA_ARG Arg1, Arg2, Arg3;
+ ULONG ArgVal3;
+ UBYTE CmpBool;
+ DV_INDEX DVIndex1, DVIndex2;
+ UWORD i;
+
+ polyBinopDispatch ++;
+ gPCDelta= 4;
+
+ NXT_ASSERT(pCode != NULL);
+ opCode = OP_CODE(pCode);
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+
+ if (opCode <= OP_XOR) // && ! OP_NEG, can't happen since it is unop
+ Status= cCmdInterpPolyBinop(opCode, Arg1, 0, Arg2, 0, Arg3, 0);
+ else if(opCode >= OP_SETIN && opCode <= OP_GETOUT)
+ Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
+ else {
+ switch (opCode)
+ {
+ case OP_CMP:
+ {
+ TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
+ if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
+ ULONG ArgVal1, ArgVal2, ArgVal3;
+ ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
+ ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
+ ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+ cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
+ PolyScalarCmp++;
+ }
+ else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1)))
+ {
+ //Compare Aggregates
+ Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
+ cCmdSetScalarValFromDataArg(Arg1, CmpBool);
+ recursiveCmp++;
+ }
+ else
+ {
+ //Compare Elements
+ Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
+ polyPolyCmp++;
+ }
+ }
+ break;
+
+ case OP_BRCMP:
+ {
+ TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
+ if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
+ ULONG ArgVal2, ArgVal3;
+ ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
+ ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
+ CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+ }
+ else //Compare Aggregates
+ Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
+
+ if (CmpBool)
+ gPCDelta = (SWORD)Arg1;
+ }
+ break;
+
+ case OP_INDEX:
+ {
+ ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0;
+
+ DVIndex2 = cCmdGetDVIndex(Arg2, 0);
+ if (ArgVal3 >= DV_ARRAY[DVIndex2].Count)
+ return (ERR_ARG);
+
+ Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3));
+ }
+ break;
+
+ case OP_ARRINIT:
+ {
+ //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length
+
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+
+ ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0;
+
+ Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3);
+ if (!IS_ERR(Status))
+ {
+ DVIndex1 = cCmdGetDVIndex(Arg1, 0);
+ if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR)
+ {
+ ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
+ TYPE_CODE TypeCode= cCmdDSType(INC_ID(Arg1));
+ for (i = 0; i < ArgVal3; i++) // could init ptr and incr by offset GM???
+ {
+ //copy Arg2 into each element of Arg1
+ cCmdSetVal(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex1, i), TypeCode, val);
+ }
+ }
+ else
+ for (i = 0; i < ArgVal3; i++) //copy Arg2 into each element of Arg1
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0);
+ }
+ }
+ break;
+
+ default:
+ {
+ //Fatal error: Unrecognized instruction
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+ break;
+ }
+ }
+ return (Status);
+}
+
+
+NXT_STATUS cCmdInterpPolyBinop(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
+{
+ NXT_STATUS Status = NO_ERR;
+ TYPE_CODE TypeCode1, TypeCode2, TypeCode3;
+ DV_INDEX DVIndex1, DVIndex2, DVIndex3;
+ ULONG ArgVal1, ArgVal2, ArgVal3;
+ float FltArgVal1, FltArgVal2, FltArgVal3;
+ UWORD Count1, Count2, Count3;
+ UWORD MinArrayCount;
+ UWORD i;
+ //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters.
+ // In practice, the logic it uses is broken, leading to some source cluster elements being ignored.
+ UBYTE AdvCluster;
+
+ void * pArg1 = NULL,
+ *pArg2 = NULL,
+ *pArg3 = NULL;
+
+ TypeCode1 = cCmdDSType(Arg1);
+ TypeCode2 = cCmdDSType(Arg2);
+ TypeCode3 = cCmdDSType(Arg3);
+
+ //Simple case, both args are scalars. Solve and return.
+ if ((!IS_AGGREGATE_TYPE(TypeCode2)) && (!IS_AGGREGATE_TYPE(TypeCode3)))
+ {
+ NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1));
+
+ pArg1 = cCmdResolveDataArg(Arg1, Offset1, NULL);
+
+ if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT || TypeCode3 == TC_FLOAT){
+ pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
+ pArg3 = cCmdResolveDataArg(Arg3, Offset3, NULL);
+ FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
+ FltArgVal3 = cCmdGetValFlt(pArg3, TypeCode3);
+ FltArgVal1 = cCmdBinopFlt(Code, FltArgVal2, FltArgVal3, TypeCode2, TypeCode3);
+ cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
+ }
+ else
+ {
+ ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
+ ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);
+ ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3);
+ cCmdSetVal(pArg1, TypeCode1, ArgVal1);
+ }
+ return Status;
+ }
+
+ //At least one of the args is an aggregate type
+
+ //
+ // Initialize Count and ArrayType local variables for each argument
+ //
+
+ if (TypeCode2 == TC_ARRAY)
+ {
+ Count2 = cCmdArrayCount(Arg2, Offset2);
+ DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
+ Offset2 = DV_ARRAY[DVIndex2].Offset;
+ }
+ else if (TypeCode2 == TC_CLUSTER)
+ {
+ Count2 = cCmdClusterCount(Arg2);
+ }
+
+ if (TypeCode3 == TC_ARRAY)
+ {
+ Count3 = cCmdArrayCount(Arg3, Offset3);
+ DVIndex3 = cCmdGetDVIndex(Arg3, Offset3);
+ Offset3 = DV_ARRAY[DVIndex3].Offset;
+ }
+ else if (TypeCode3 == TC_CLUSTER)
+ {
+ Count3 = cCmdClusterCount(Arg3);
+ }
+
+
+ if (TypeCode1 == TC_ARRAY)
+ {
+ //If the destination is an array, make sure it has enough memory to hold the result
+ if ((TypeCode2 == TC_ARRAY) && (TypeCode3 == TC_ARRAY))
+ {
+ if (Count2 < Count3)
+ MinArrayCount = Count2;
+ else
+ MinArrayCount = Count3;
+ }
+ else if (TypeCode2 == TC_ARRAY)
+ MinArrayCount = Count2;
+ else if (TypeCode3 == TC_ARRAY)
+ MinArrayCount = Count3;
+ else
+ {
+ //If output is an array, but no sources are arrays, that's a fatal error!
+ NXT_BREAK;
+ return (ERR_ARG);
+ }
+
+ //Make sure the destination array is the proper size to hold the result
+ Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount);
+ if (IS_ERR(Status))
+ return Status;
+
+ Count1 = MinArrayCount;
+ DVIndex1 = cCmdGetDVIndex(Arg1, Offset1);
+ Offset1 = DV_ARRAY[DVIndex1].Offset;
+ AdvCluster = FALSE;
+ }
+ else if (TypeCode1 == TC_CLUSTER)
+ {
+ Count1 = cCmdClusterCount(Arg1);
+ AdvCluster = TRUE;
+ }
+
+ //Advance aggregate args to first sub-element for next call
+ if (IS_AGGREGATE_TYPE(TypeCode1))
+ Arg1 = INC_ID(Arg1);
+ if (IS_AGGREGATE_TYPE(TypeCode2))
+ Arg2 = INC_ID(Arg2);
+ if (IS_AGGREGATE_TYPE(TypeCode3))
+ Arg3 = INC_ID(Arg3);
+
+ //
+ // Loop through the sub-elements of aggregate arguments.
+ // Call cCmdInterpPolyBinop recursively with simpler type.
+ //
+
+ for (i = 0; i < Count1; i++)
+ {
+ Status = cCmdInterpPolyBinop(Code, Arg1, Offset1, Arg2, Offset2, Arg3, Offset3);
+ if (IS_ERR(Status))
+ return Status;
+
+ //Advance aggregate args to next sub-element
+ if (TypeCode1 == TC_ARRAY)
+ Offset1 += DV_ARRAY[DVIndex1].ElemSize;
+ else if ((TypeCode1 == TC_CLUSTER) && AdvCluster)
+ Arg1 = cCmdNextDSElement(Arg1);
+
+ if (TypeCode2 == TC_ARRAY)
+ Offset2 += DV_ARRAY[DVIndex2].ElemSize;
+ else if ((TypeCode2 == TC_CLUSTER) && AdvCluster)
+ Arg2 = cCmdNextDSElement(Arg2);
+
+ if (TypeCode3 == TC_ARRAY)
+ Offset3 += DV_ARRAY[DVIndex3].ElemSize;
+ else if ((TypeCode3 == TC_CLUSTER) && AdvCluster)
+ Arg3 = cCmdNextDSElement(Arg3);
+ }
+
+ return Status;
+}
+
+
+ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE LeftType, TYPE_CODE RightType)
+{
+ UBYTE opCode;
+
+ opCode = OP_CODE((&Code));
+
+ switch (opCode)
+ {
+ case OP_ADD:
+ {
+ return LeftOp + RightOp;
+ }
+
+ case OP_SUB:
+ {
+ return LeftOp - RightOp;
+ }
+
+ case OP_MUL:
+ {
+ return LeftOp * RightOp;
+ }
+
+ case OP_DIV:
+ {
+ //Catch divide-by-zero for a portable, well-defined result.
+ //(x / 0) = 0. Thus Spake LOTHAR!! (It's technical.)
+ if (RightOp == 0)
+ return 0;
+
+ if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType))
+ return ((SLONG)LeftOp) / ((SLONG)RightOp);
+ else if (IS_SIGNED_TYPE(LeftType))
+ return ((SLONG)LeftOp) / RightOp;
+ else if (IS_SIGNED_TYPE(RightType))
+ return LeftOp / ((SLONG)RightOp);
+ else
+ return LeftOp / RightOp;
+ }
+
+ case OP_MOD:
+ {
+ //As with OP_DIV, make sure (x % 0) = x is well-defined
+ if (RightOp == 0)
+ return (LeftOp);
+
+ if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType))
+ return ((SLONG)LeftOp) % ((SLONG)RightOp);
+ else if (IS_SIGNED_TYPE(LeftType))
+ return ((SLONG)LeftOp) % RightOp;
+ else if (IS_SIGNED_TYPE(RightType))
+ return LeftOp % ((SLONG)RightOp);
+ else
+ return LeftOp % RightOp;
+ }
+
+ case OP_AND:
+ {
+ return (LeftOp & RightOp);
+ }
+
+ case OP_OR:
+ {
+ return (LeftOp | RightOp);
+ }
+
+ case OP_XOR:
+ {
+ return ((LeftOp | RightOp) & (~(LeftOp & RightOp)));
+ }
+
+ case OP_CMP:
+ {
+ return cCmdCompare(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType);
+ }
+
+ default:
+ {
+ //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
+ NXT_BREAK;
+ return 0;
+ }
+ }
+}
+
+
+float cCmdBinopFlt(CODE_WORD const Code, float LeftOp, float RightOp, TYPE_CODE LeftType, TYPE_CODE RightType)
+{
+ UBYTE opCode;
+
+ opCode = OP_CODE((&Code));
+
+ switch (opCode)
+ {
+ case OP_ADD:
+ {
+ return LeftOp + RightOp;
+ }
+
+ case OP_SUB:
+ {
+ return LeftOp - RightOp;
+ }
+
+ case OP_MUL:
+ {
+ return LeftOp * RightOp;
+ }
+
+ case OP_DIV:
+ {
+ //Catch divide-by-zero for a portable, well-defined result.
+ //(x / 0) = 0. Thus Spake LOTHAR!! (It's technical.)
+ if (RightOp == 0)
+ return 0;
+
+ return LeftOp / RightOp;
+ }
+
+ case OP_MOD:
+ {
+ //As with OP_DIV, make sure (x % 0) = x is well-defined
+ if (RightOp == 0)
+ return (LeftOp);
+
+ return (SLONG)LeftOp % (SLONG)RightOp;
+ }
+
+ case OP_AND:
+ {
+ return ((SLONG)LeftOp & (SLONG)RightOp);
+ }
+
+ case OP_OR:
+ {
+ return ((SLONG)LeftOp | (SLONG)RightOp);
+ }
+
+ case OP_XOR:
+ {
+ return (((SLONG)LeftOp | (SLONG)RightOp) & (~((SLONG)LeftOp & (SLONG)RightOp)));
+ }
+
+ case OP_CMP:
+ {
+ return cCmdCompareFlt(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType);
+ }
+
+ default:
+ {
+ //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
+ NXT_BREAK;
+ return 0;
+ }
+ }
+}
+
+NXT_STATUS cCmdInterpNoArg(CODE_WORD * const pCode)
+{
+ //Fatal error: Unrecognized instruction (no current opcodes have zero instructions)
+ NXT_BREAK;
+ return (ERR_INSTR);
+}
+
+NXT_STATUS cCmdInterpShortError(CODE_WORD * const pCode)
+{
+ //Fatal error: Unrecognized instruction (no current opcodes have zero instructions)
+ NXT_BREAK;
+ return (ERR_INSTR);
+}
+
+NXT_STATUS cCmdInterpShortSubCall(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ DATA_ARG Arg1, Arg2;
+
+ gPCDelta= 2;
+ Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
+ Arg2 = GetDataArg(pCode[1]);
+ NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1));
+ NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1));
+
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg2));
+
+ *((CLUMP_ID *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;
+
+ cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
+ cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1); //Add callee to RunQ
+
+ Status = CLUMP_SUSPEND;
+
+ return Status;
+}
+
+ULONG moveSameInt= 0, moveDiffInt= 0, moveFloat= 0, moveArrInt= 0, moveOther= 0;
+NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2)
+{
+ NXT_STATUS Status;
+ DS_TOC_ENTRY *TOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1],
+ *TOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
+ TYPE_CODE tc1= TOC1Ptr->TypeCode, tc2= TOC2Ptr->TypeCode;
+ void *pArg1, *pArg2;
+
+ if(tc1 <= TC_LAST_INT_SCALAR && tc2 <= TC_LAST_INT_SCALAR)
+ {
+ // if tc1 == tc2, do long, byte, then word assignment
+ if(tc1 == tc2)
+ {
+ moveSameInt++;
+ pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
+ pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
+ if(tc1 >= TC_ULONG)
+ *(ULONG*)pArg1= *(ULONG*)pArg2;
+ else if(tc1 <= TC_SBYTE)
+ *(UBYTE*)pArg1= *(UBYTE*)pArg2;
+ else
+ *(UWORD*)pArg1= *(UWORD*)pArg2;
+ Status= NO_ERR;
+ }
+ else
+ {
+ moveDiffInt++;
+ ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
+ cCmdSetScalarValFromDataArg(Arg1, val);
+ Status= NO_ERR;
+ }
+ }
+ else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) { // may also need to speed up float to int and int to float conversions
+ moveFloat++;
+ pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
+ pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
+ *(float*)pArg1= *(float*)pArg2;
+ Status= NO_ERR;
+ }
+ //!!! Optimized move for arrays of ints.
+ else if ((tc1 == TC_ARRAY) && (tc2 == TC_ARRAY)
+ && ((TOC1Ptr+1)->TypeCode <= TC_LAST_INT_SCALAR)
+ && ((TOC1Ptr+1)->TypeCode == (TOC2Ptr+1)->TypeCode))
+ {
+ ULONG Count;
+ moveArrInt++;
+ Count = cCmdArrayCount(Arg2, 0);
+ Status = cCmdDSArrayAlloc(Arg1, 0, Count);
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+
+ memmove(pArg1, pArg2, Count * cCmdSizeOf((TOC1Ptr+1)->TypeCode));
+ }
+ else { // if ((tc1 == TC_CLUSTER) && (tc2 == TC_CLUSTER))
+ moveOther++;
+ Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0);
+ }
+ return Status;
+}
+
+
+NXT_STATUS cCmdInterpShortMove(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ DATA_ARG Arg1, Arg2;
+
+ Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
+ Arg2 = GetDataArg(pCode[1]);
+ Status= cCmdMove(Arg1, Arg2);
+
+ gPCDelta= 2;
+ return Status;
+}
+
+NXT_STATUS cCmdInterpShortAcquire(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ DATA_ARG Arg1;
+
+ gPCDelta= 1;
+ Arg1 = GetDataArg(SHORT_ARG(pCode));
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);
+
+ Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
+
+ return Status;
+}
+
+NXT_STATUS cCmdInterpShortRelease(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status;
+ DATA_ARG Arg1;
+
+ gPCDelta= 1;
+ Arg1 = GetDataArg(SHORT_ARG(pCode));
+ NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);
+
+ Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
+
+ return Status;
+}
+
+
+//OP_SETOUT gets it's own interpreter function because it is relatively complex
+// (called from cCmdInterpOther())
+//This also serves as a convenient breakpoint stop for investigating output module behavior
+NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode)
+{
+ TYPE_CODE TypeCodeField, TypeCodeSrc, TypeCodePortArg;
+ void *pField = NULL,
+ *pSrc = NULL,
+ *pPort = NULL;
+ DS_ELEMENT_ID PortArg;
+ UWORD PortCount, InstrSize;
+ ULONG Port, FieldTableIndex, i, j;
+ DV_INDEX DVIndex;
+
+ //Arg1 = InstrSize
+ //Arg2 = port number or list of ports
+ //Arg3 and beyond = FieldID, src DSID tuples
+
+ //Calculate number of tuples
+ //!!! Might want to throw ERR_INSTR if instrSize and tuples don't check out
+ InstrSize = (pCode[1] / 2);
+
+ //Second argument may specify a single port or an array list.
+ //Figure out which and resolve accordingly.
+ PortArg = pCode[2];
+ TypeCodePortArg = cCmdDSType(PortArg);
+
+ if (TypeCodePortArg == TC_ARRAY)
+ {
+ DVIndex = cCmdGetDVIndex(PortArg, 0);
+ PortCount = cCmdArrayCount(PortArg, 0);
+ }
+ else
+ PortCount = 1;
+
+ //For each port, process all the tuples
+ for (i = 0; i < PortCount; i++)
+ {
+ if (TypeCodePortArg == TC_ARRAY)
+ {
+ pPort = (UBYTE*)cCmdResolveDataArg(INC_ID(PortArg), ARRAY_ELEM_OFFSET(DVIndex, i), NULL);
+ Port = cCmdGetVal(pPort, cCmdDSType(INC_ID(PortArg)));
+ }
+ else
+ {
+ Port = cCmdGetScalarValFromDataArg(PortArg, 0);
+ }
+
+ //If user specified a valid port, process the tuples. Else, this port is a no-op
+ if (Port < NO_OF_OUTPUTS)
+ {
+ for (j = 3; j < InstrSize; j += 2)
+ {
+ FieldTableIndex = (Port * IO_OUT_FPP) + pCode[j];
+ pSrc = cCmdResolveDataArg(pCode[j + 1], 0, &TypeCodeSrc);
+
+ //If FieldTableIndex is valid, go ahead and set the value
+ if (FieldTableIndex < IO_OUT_FIELD_COUNT)
+ {
+ pField = IO_PTRS[MOD_OUTPUT][FieldTableIndex];
+ TypeCodeField = IO_TYPES[MOD_OUTPUT][FieldTableIndex];
+ cCmdSetVal(pField, TypeCodeField, cCmdGetVal(pSrc, TypeCodeSrc));
+ }
+ //Else, compiler is nutso! Return fatal error.
+ else
+ return (ERR_INSTR);
+ }
+ }
+ }
+
+ return (NO_ERR);
+}
+
+
+NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode)
+{
+ NXT_STATUS Status = NO_ERR;
+ UBYTE opCode;
+ DATA_ARG Arg1, Arg2, Arg3, Arg4, Arg5;
+ TYPE_CODE TypeCode1, TypeCode2, TypeCode3, TypeCode5;
+ ULONG ArgVal2, ArgVal3, ArgVal4, ArgVal5;
+ UWORD ArrayCount1, ArrayCount2, ArrayCount3, ArrayCount4;
+ UWORD MinCount;
+ UWORD i,j;
+ DV_INDEX DVIndex1, DVIndex2, DVIndex4,TmpDVIndex;
+ UWORD SrcCount;
+ DS_ELEMENT_ID TmpDSID;
+ UWORD DstIndex;
+ UWORD Size;
+ UWORD Offset;
+
+ void *pArg1 = NULL;
+ void *pArg2 = NULL;
+ void *pArg3 = NULL;
+ void *pArg5 = NULL;
+
+ NXT_ASSERT(pCode != NULL);
+
+ ULONG sz= INSTR_SIZE(*(UWORD*)pCode);
+ if (sz == VAR_INSTR_SIZE)
+ sz = ((UWORD*)pCode)[1];
+ gPCDelta= sz/2; // advance words, sz is in bytes
+
+ opCode = OP_CODE(pCode);
+
+ switch (opCode)
+ {
+
+ case OP_REPLACE:
+ {
+ //Arg1 - Dst
+ //Arg2 - Src
+ //Arg3 - Index
+ //Arg4 - New val / array of vals
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Arg4 = pCode[4];
+
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
+
+ //Copy Src to Dst
+ //!!! Could avoid full data copy if we knew which portion to overwrite
+ if (Arg1 != Arg2)
+ {
+ Status= cCmdMove(Arg1, Arg2);
+ if (IS_ERR(Status))
+ return Status;
+ }
+
+ DVIndex1 = cCmdGetDVIndex(Arg1, 0);
+ //Copy new val to Dst
+ if (Arg3 != NOT_A_DS_ID)
+ {
+ pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3);
+ ArgVal3 = cCmdGetVal(pArg3, TypeCode3);
+ }
+ else
+ {
+ //Index input unwired
+ ArgVal3 = 0;
+ }
+
+ ArrayCount1 = cCmdArrayCount(Arg1, 0);
+ //Bounds check
+ //If array index (ArgVal3) is out of range, just pass out the copy of Src (effectively no-op)
+ if (ArgVal3 >= ArrayCount1)
+ return (NO_ERR);
+
+ if (cCmdDSType(Arg4) != TC_ARRAY)
+ {
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3), Arg4, 0);
+ if (IS_ERR(Status))
+ return Status;
+ }
+ else
+ {
+ DVIndex4 = cCmdGetDVIndex(Arg4, 0);
+
+ ArrayCount4 = cCmdArrayCount(Arg4, 0);
+ if (ArrayCount1 - ArgVal3 < ArrayCount4)
+ MinCount = (UWORD)(ArrayCount1 - ArgVal3);
+ else
+ MinCount = ArrayCount4;
+
+ for (i = 0; i < MinCount; i++)
+ {
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3 + i), INC_ID(Arg4), ARRAY_ELEM_OFFSET(DVIndex4, i));
+ if (IS_ERR(Status))
+ return Status;
+ }
+ }
+ }
+ break;
+
+ case OP_ARRSUBSET:
+ {
+ //Arg1 - Dst
+ //Arg2 - Src
+ //Arg3 - Index
+ //Arg4 - Length
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Arg4 = pCode[4];
+
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
+
+ ArrayCount2 = cCmdArrayCount(Arg2, 0);
+
+ if (Arg3 != NOT_A_DS_ID)
+ ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
+ else //Index input unwired
+ ArgVal3 = 0;
+
+ if (Arg4 != NOT_A_DS_ID)
+ ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
+ else //Length input unwired, set to "rest"
+ ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3);
+
+ //Bounds check
+ if (ArgVal3 > ArrayCount2)
+ {
+ //Illegal range - return empty subset
+ Status = cCmdDSArrayAlloc(Arg1, 0, 0);
+ return Status;
+ }
+
+ //Set MinCount to "rest"
+ MinCount = (UWORD)(ArrayCount2 - ArgVal3);
+
+ // Copy "Length" if it is less than "rest"
+ if (ArgVal4 < (ULONG)MinCount)
+ MinCount = (UWORD)ArgVal4;
+
+ //Allocate Dst array
+ Status = cCmdDSArrayAlloc(Arg1, 0, MinCount);
+ if (IS_ERR(Status))
+ return Status;
+
+ DVIndex1 = cCmdGetDVIndex(Arg1, 0);
+ DVIndex2 = cCmdGetDVIndex(Arg2, 0);
+
+ //Move src subset to dst
+ for (i = 0; i < MinCount; i++)
+ {
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3 + i));
+ if (IS_ERR(Status))
+ return Status;
+ }
+ }
+ break;
+
+ case OP_STRSUBSET:
+ {
+ //Arg1 - Dst
+ //Arg2 - Src
+ //Arg3 - Index
+ //Arg4 - Length
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Arg4 = pCode[4];
+
+ NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);
+ NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE);
+
+ ArrayCount2 = cCmdArrayCount(Arg2, 0);
+
+ //Remove NULL from Count
+ ArrayCount2--;
+
+ if (Arg3 != NOT_A_DS_ID)
+ ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
+ else //Index input unwired
+ ArgVal3 = 0;
+
+ if (Arg4 != NOT_A_DS_ID)
+ ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
+ else //Length input unwired, set to "rest"
+ ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3);
+
+ //Bounds check
+ if (ArgVal3 > ArrayCount2)
+ {
+ //Illegal range - return empty string
+ Status = cCmdDSArrayAlloc(Arg1, 0, 1);
+ if (!IS_ERR(Status))
+ {
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ *((UBYTE *)pArg1) = '\0';
+ }
+ return Status;
+ }
+
+ //Set MinCount to "rest"
+ MinCount = (UWORD)(ArrayCount2 - ArgVal3);
+
+ // Copy "Length" if it is less than "rest"
+ if (ArgVal4 < (ArrayCount2 - ArgVal3))
+ MinCount = (UWORD)ArgVal4;
+
+ //Allocate Dst array
+ Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(MinCount + 1));
+ if (IS_ERR(Status))
+ return Status;
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+
+ //Move src subset to dst
+ memmove((UBYTE *)pArg1, (UBYTE *)pArg2 + ArgVal3, MinCount);
+
+ //Append NULL terminator to Dst
+ *((UBYTE *)pArg1 + MinCount) = '\0';
+
+ }
+ break;
+
+ case OP_SETOUT:
+ {
+ Status = cCmdExecuteSetOut(pCode);
+ }
+ break;
+
+ case OP_ARRBUILD:
+ {
+ // Arg1 - Instruction Size in bytes
+ // Arg2 - Dst
+ // Arg3-N - Srcs
+
+ Arg2 = pCode[2];
+
+ NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
+
+ //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst)
+ //!!! Argument access like this is potentially unsafe.
+ //A function/macro which checks proper encoding would be better
+ SrcCount = (pCode[1] / 2) - 3;
+
+ //Calculate Dst array count
+ ArrayCount2 = 0;
+ for (i = 0; i < SrcCount; i++)
+ {
+ TmpDSID = pCode[3 + i];
+ NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID));
+
+ //If the type descriptors are the same, then the input is an array, not a single element
+ if (cCmdCompareDSType(Arg2, TmpDSID))
+ {
+ NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);
+ ArrayCount2 += cCmdArrayCount(TmpDSID, 0);
+ }
+ else
+ {
+ //Assert that the output is an array of this input type
+ NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID));
+ ArrayCount2++;
+ }
+ }
+
+ //Allocate Dst array
+ Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2);
+ if (IS_ERR(Status))
+ return Status;
+
+ DVIndex2 = cCmdGetDVIndex(Arg2, 0);
+
+ //Move Src(s) to Dst
+ DstIndex = 0;
+ for (i = 0; i < SrcCount; i++)
+ {
+ TmpDSID = pCode[3 + i];
+
+ //If the type descriptors are the same, then the input is an array, not a single element
+ if (cCmdCompareDSType(Arg2, TmpDSID))
+ {
+ NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);
+ TmpDVIndex = cCmdGetDVIndex(TmpDSID, 0);
+ // if flat, use memmove, otherwise this stuff
+ if(cCmdDSType(INC_ID(TmpDSID)) <= TC_LAST_INT_SCALAR)
+ {
+ memmove(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), VarsCmd.pDataspace + DV_ARRAY[TmpDVIndex].Offset, (UWORD)(DV_ARRAY[TmpDVIndex].ElemSize * DV_ARRAY[TmpDVIndex].Count));
+ DstIndex += DV_ARRAY[TmpDVIndex].Count;
+ }
+ else
+ for (j = 0; j < DV_ARRAY[TmpDVIndex].Count; j++)
+ {
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), INC_ID(TmpDSID), ARRAY_ELEM_OFFSET(TmpDVIndex, j));
+ if (IS_ERR(Status))
+ return Status;
+ DstIndex++;
+ }
+ }
+ else
+ {
+ //Assert that the output is an array of this input type
+ NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID));
+ Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), TmpDSID, 0);
+ if (IS_ERR(Status))
+ return Status;
+ DstIndex++;
+ }
+ }
+
+ NXT_ASSERT(DstIndex == ArrayCount2);
+ }
+ break;
+
+ case OP_STRCAT:
+ {
+ // Arg1 - Instruction Size in bytes
+ // Arg2 - Dst
+ // Arg3-N - Srcs
+
+ Arg2 = pCode[2];
+
+ //Make sure Dst arg is a string
+ NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE);
+
+ //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst)
+ //!!! Argument access like this is potentially unsafe.
+ //A function/macro which checks proper encoding would be better
+ SrcCount = (pCode[1] / 2) - 3;
+
+ //Calculate Dst array count
+ ArrayCount2 = 0;
+ for (i = 0; i < SrcCount; i++)
+ {
+ TmpDSID = pCode[3 + i];
+ NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID));
+
+ //Make sure Src arg is a string
+ //!!! Type checks here should be richer to allow array of strings as input (match LabVIEW behavior)
+ NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);
+
+ if (cCmdDSType(INC_ID(TmpDSID)) != TC_UBYTE)
+ {
+ NXT_BREAK;
+ return ERR_ARG;
+ }
+
+ ArrayCount3 = cCmdArrayCount(TmpDSID, 0);
+ NXT_ASSERT(ArrayCount3 > 0);
+ //Subtract NULL terminator from Src array count
+ ArrayCount3--;
+
+ //Increase Dst array count by Src array count
+ ArrayCount2 += ArrayCount3;
+ }
+
+ //Add room for NULL terminator
+ ArrayCount2++;
+
+ //Allocate Dst array
+ Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2);
+ if (IS_ERR(Status))
+ return Status;
+
+ //Move Src(s) to Dst
+ DstIndex = 0;
+ pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
+ for (i = 0; i < SrcCount; i++)
+ {
+ TmpDSID = pCode[3 + i];
+
+ pArg3 = cCmdResolveDataArg(TmpDSID, 0, NULL);
+
+ ArrayCount3 = cCmdArrayCount(TmpDSID, 0);
+ NXT_ASSERT(ArrayCount3 > 0);
+ //Subtract NULL terminator from Src array count
+ ArrayCount3--;
+
+ memmove((UBYTE *)pArg2 + DstIndex, pArg3, ArrayCount3);
+ DstIndex += ArrayCount3;
+ }
+
+ //Append NULL terminator to Dst
+ *((UBYTE *)pArg2 + DstIndex) = '\0';
+ DstIndex++;
+
+ NXT_ASSERT(DstIndex == ArrayCount2);
+ }
+ break;
+
+ case OP_UNFLATTEN:
+ {
+ //Arg1 - Dst
+ //Arg2 - Err (output)
+ //Arg3 - Src (byte stream)
+ //Arg4 - Type
+
+ //The Type arg is a preallocated structure of the exact size you
+ //want to unflatten into. This allows us to support unflattening arbitrary types.
+
+ //!!! Currently, both outputs must have valid destinations.
+ // It would be trivial to handle NOT_A_DS_ID to avoid dummy
+ // allocations when outputs are unused.
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Arg4 = pCode[4];
+
+ //Move Type template to Dst
+ //This provides a default value for Dst and makes sure Dst is properly sized
+ Status= cCmdMove(Arg1, Arg4);
+ if (IS_ERR(Status))
+ return Status;
+
+ //Resolve error data pointer
+ pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2);
+
+ //Make sure Arg3 is a String
+ NXT_ASSERT(cCmdDSType(Arg3) == TC_ARRAY);
+ NXT_ASSERT(cCmdDSType(INC_ID(Arg3)) == TC_UBYTE);
+
+ ArrayCount3 = cCmdArrayCount(Arg3, 0);
+ //Take NULL terminator out of count
+ ArrayCount3--;
+
+ Size = cCmdCalcFlattenedSize(Arg4, 0);
+
+ //Check that we have a proper type template to unflatten into
+ if (ArrayCount3 == Size)
+ {
+ pArg3 = cCmdResolveDataArg(Arg3, 0, NULL);
+ Offset = 0;
+ Status = cCmdUnflattenFromByteArray(pArg3, &Offset, Arg1, 0);
+
+ //!!! Status ignored from cCmdUnflattenFromByteArray
+ // If future revisions of this function provide better error checking,
+ // Err arg should be conditionally set based on the result.
+ //Unflatten succeeded; set Err arg to FALSE
+ cCmdSetVal(pArg2, TypeCode2, FALSE);
+
+ NXT_ASSERT(Offset == Size);
+ }
+ else
+ {
+ //Unflatten failed; set Err arg to TRUE
+ cCmdSetVal(pArg2, TypeCode2, TRUE);
+ }
+ }
+ break;
+
+ case OP_STRINGTONUM:
+ {
+ float ArgValF;
+ SLONG decimals= 0;
+ UBYTE cont= TRUE;
+ // Arg1 - Dst number (output)
+ // Arg2 - Offset past match (output)
+ // Arg3 - Src string
+ // Arg4 - Offset
+ // Arg5 - Default (type/value)
+
+ //!!! Currently, both outputs must have valid destinations.
+ // It would be trivial to handle NOT_A_DS_ID to avoid dummy
+ // allocations when outputs are unused.
+
+ Arg1 = pCode[1];
+ Arg2 = pCode[2];
+ Arg3 = pCode[3];
+ Arg4 = pCode[4];
+ Arg5 = pCode[5];
+
+ pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1);
+ pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3);
+
+ if (Arg4 != NOT_A_DS_ID)
+ ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
+ else //Offset input unwired
+ ArgVal4 = 0;
+
+ if (Arg5 != NOT_A_DS_ID)
+ {
+ pArg5 = cCmdResolveDataArg(Arg5, 0, &TypeCode5);
+ ArgVal5 = cCmdGetVal(pArg5, TypeCode5);
+ }
+ else //Default input unwired
+ {
+ ArgVal5 = 0;
+ }
+
+ //Read number from string
+ if (sscanf(((PSZ)pArg3 + ArgVal4), "%f", &ArgValF) == 1)
+ {
+ i = (UWORD)ArgVal4;
+ //Scan until we see the number, consumes negative sign too
+ while ((((UBYTE *)pArg3)[i] < '0') || (((UBYTE *)pArg3)[i] > '9'))
+ i++;
+
+ //Scan until we get past the number and no more than one decimal
+ while (cont) {
+ if ((((UBYTE *)pArg3)[i] >= '0') && (((UBYTE *)pArg3)[i] <= '9'))
+ i++;
+ else if(((UBYTE *)pArg3)[i] == '.' && !decimals) {
+ i++;
+ decimals++;
+ }
+ else
+ cont= FALSE;
+ }
+ ArgVal2 = i;
+ }
+ else
+ {
+ //Number wasn't found in string, use defaults
+ ArgValF = ArgVal5;
+ ArgVal2 = 0;
+ }
+
+ //Set outputs
+ cCmdSetValFlt(pArg1, TypeCode1, ArgValF);
+ cCmdSetScalarValFromDataArg(Arg2, ArgVal2);
+ }
+ break;
+
+ default:
+ {
+ //Fatal error: Unrecognized instruction
+ NXT_BREAK;
+ Status = ERR_INSTR;
+ }
+ break;
+ }
+
+ return (Status);
+}
+
+
+//
+//Support functions for lowspeed (I2C devices, i.e. ultrasonic sensor) communications
+//
+
+//Simple lookup table for pMapLowSpeed->ChannelState[Port] values
+//This is used to keep VM status code handling consistent
+//...and ChannelState gives us too much information, anyway...
+static const NXT_STATUS MapLStoVMStat[6] =
+{
+ NO_ERR, //LOWSPEED_IDLE,
+ STAT_COMM_PENDING, //LOWSPEED_INIT,
+ STAT_COMM_PENDING, //LOWSPEED_LOAD_BUFFER,
+ STAT_COMM_PENDING, //LOWSPEED_COMMUNICATING,
+ ERR_COMM_BUS_ERR, //LOWSPEED_ERROR,
+ STAT_COMM_PENDING, //LOWSPEED_DONE (really means c_lowspeed state machine is resetting)
+};
+
+
+//cCmdLSCheckStatus
+//Check lowspeed port status, optionally returning bytes available in the buffer for reading
+NXT_STATUS cCmdLSCheckStatus(UBYTE Port)
+{
+ if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ return (ERR_COMM_CHAN_INVALID);
+ }
+
+ //If port is not configured properly ahead of time, report that error
+ //!!! This seems like the right policy, but may restrict otherwise valid read operations...
+ if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
+ || !(pMapInput->Inputs[Port].InvalidData == FALSE))
+ {
+ return (ERR_COMM_CHAN_NOT_READY);
+ }
+
+ return (MapLStoVMStat[pMapLowSpeed->ChannelState[Port]]);
+}
+
+//cCmdLSCalcBytesReady
+//Calculate true number of bytes available in the inbound LS buffer
+UBYTE cCmdLSCalcBytesReady(UBYTE Port)
+{
+ SLONG Tmp;
+
+ //Expect callers to validate Port, but short circuit here to be safe.
+ if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
+ return 0;
+
+ //Normally, bytes available is a simple difference.
+ Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr;
+
+ //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage...
+ if (Tmp < 0)
+ Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr));
+
+ return (UBYTE)(Tmp);
+}
+
+//cCmdLSWrite
+//Write BufLength bytes into specified port's lowspeed buffer and kick off comm process to device
+NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength)
+{
+ if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ return (ERR_COMM_CHAN_INVALID);
+ }
+
+ if (BufLength > SIZE_OF_LSBUF || ResponseLength > SIZE_OF_LSBUF)
+ {
+ return (ERR_INVALID_SIZE);
+ }
+
+ //Only start writing process if port is properly configured and c_lowspeed module is ready
+ if ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
+ && (pMapInput->Inputs[Port].InvalidData == FALSE)
+ && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR))
+ {
+ pMapLowSpeed->OutBuf[Port].InPtr = 0;
+ pMapLowSpeed->OutBuf[Port].OutPtr = 0;
+
+ memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength);
+ pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength;
+
+ pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength;
+
+ pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT;
+ pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port);
+
+ return (NO_ERR);
+ }
+ else
+ {
+ //!!! Would be more consistent to return STAT_COMM_PENDING if c_lowspeed is busy
+ return (ERR_COMM_CHAN_NOT_READY);
+ }
+}
+
+
+//cCmdLSRead
+//Read BufLength bytes from specified port's lowspeed buffer
+NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf)
+{
+ UBYTE BytesReady, BytesToRead;
+
+ if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
+ {
+ return (ERR_COMM_CHAN_INVALID);
+ }
+
+ if (BufLength > SIZE_OF_LSBUF)
+ {
+ return (ERR_INVALID_SIZE);
+ }
+
+ BytesReady = cCmdLSCalcBytesReady(Port);
+
+ if (BufLength > BytesReady)
+ {
+ return (ERR_COMM_CHAN_NOT_READY);
+ }
+
+ BytesToRead = BufLength;
+
+ //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning
+ if (pMapLowSpeed->InBuf[Port].OutPtr + BytesToRead >= SIZE_OF_LSBUF)
+ {
+ BytesToRead = SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr;
+ memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead);
+ pMapLowSpeed->InBuf[Port].OutPtr = 0;
+ pBuf += BytesToRead;
+ BytesToRead = BufLength - BytesToRead;
+ }
+
+ memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead);
+ pMapLowSpeed->InBuf[Port].OutPtr += BytesToRead;
+
+ return (NO_ERR);
+}
+
+
+//
+//Wrappers for OP_SYSCALL
+//
+
+//
+//cCmdWrapFileOpenRead
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 return
+//ArgV[2]: Filename, CStr
+//ArgV[3]: Length, U32 return
+NXT_STATUS cCmdWrapFileOpenRead(UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ LStatus = pMapLoader->pFunc(OPENREAD, ArgV[2], NULL, (ULONG *)ArgV[3]);
+
+ //Add entry into FileHandleTable
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'r';
+ strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
+ }
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+ //File handle in low byte of LStatus
+ *(ArgV[1]) = LOADER_HANDLE(LStatus);
+
+ return NO_ERR;
+}
+
+//cCmdWrapFileOpenWrite
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 return
+//ArgV[2]: Filename, CStr
+//ArgV[3]: Length, U32 return
+NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ LStatus = pMapLoader->pFunc(OPENWRITEDATA, ArgV[2], NULL, (ULONG *)ArgV[3]);
+
+ //Add entry into FileHandleTable
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w';
+ strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
+ }
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+ //File handle in low byte of LStatus
+ *(ArgV[1]) = LOADER_HANDLE(LStatus);
+
+ return NO_ERR;
+}
+
+//cCmdWrapFileOpenAppend
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 return
+//ArgV[2]: Filename, CStr
+//ArgV[3]: Length Remaining, U32 return
+NXT_STATUS cCmdWrapFileOpenAppend(UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ LStatus = pMapLoader->pFunc(OPENAPPENDDATA, ArgV[2], NULL, (ULONG *)ArgV[3]);
+
+ //Add entry into FileHandleTable
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w';
+ strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
+ }
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+ //File handle in low byte of LStatus
+ *(ArgV[1]) = LOADER_HANDLE(LStatus);
+
+ return NO_ERR;
+}
+
+//cCmdWrapFileRead
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 in/out
+//ArgV[2]: Buffer, CStr out
+//ArgV[3]: Length, U32 in/out
+NXT_STATUS cCmdWrapFileRead(UBYTE * ArgV[])
+{
+ NXT_STATUS Status = NO_ERR;
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ //Size Buffer to Length
+ //Add room for null terminator to length
+ Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(*(ULONG *)ArgV[3] + 1));
+ if (IS_ERR(Status))
+ return Status;
+
+ ArgV[2] = cCmdDVPtr(DVIndex);
+ LStatus = pMapLoader->pFunc(READ, ArgV[1], ArgV[2], (ULONG *)ArgV[3]);
+
+ //Tack on NULL terminator
+ //Note that loader code may have adjusted length (*ArgV[3]) if all requested data was not available
+ //!!! Better solution would be to resize buffer to new length + 1,
+ // but then you must also be wary of side effects if resize allocation fails!
+ *(ArgV[2] + *(ULONG *)ArgV[3]) = '\0';
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+ //File handle in low byte of LStatus
+ *(ArgV[1]) = LOADER_HANDLE(LStatus);
+
+ return Status;
+}
+
+//cCmdWrapFileWrite
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 in/out
+//ArgV[2]: Buffer, CStr
+//ArgV[3]: Length, U32 return
+NXT_STATUS cCmdWrapFileWrite(UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ LStatus = pMapLoader->pFunc(WRITE, ArgV[1], ArgV[2], (ULONG *)ArgV[3]);
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+ //File handle in low byte of LStatus
+ *(ArgV[1]) = LOADER_HANDLE(LStatus);
+
+ return NO_ERR;
+}
+
+//cCmdWrapFileClose
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8
+NXT_STATUS cCmdWrapFileClose(UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+
+ //!!! This bounds check also exists in dLoaderCloseHandle(), but we provide an explicit error code
+ if (*(ArgV[1]) >= MAX_HANDLES)
+ {
+ *((UWORD *)ArgV[0]) = ILLEGALHANDLE;
+ return NO_ERR;
+ }
+
+ LStatus = pMapLoader->pFunc(CLOSE, ArgV[1], NULL, NULL);
+
+ //Clear entry in FileHandleTable
+ memset(VarsCmd.FileHandleTable[*(ArgV[1])], 0, FILENAME_LENGTH + 2);
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+
+ return NO_ERR;
+}
+
+//cCmdWrapFileResolveHandle
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: File Handle, U8 return
+//ArgV[2]: Write Handle?, Bool return
+//ArgV[3]: Filename, CStr
+NXT_STATUS cCmdWrapFileResolveHandle (UBYTE * ArgV[])
+{
+ UBYTE i;
+ DV_INDEX DVIndex;
+
+ //Resolve array argument
+ DVIndex = *(DV_INDEX *)(ArgV[3]);
+ ArgV[3] = cCmdDVPtr(DVIndex);
+
+ for (i = 0; i < MAX_HANDLES; i++)
+ {
+ if (strcmp((PSZ)(ArgV[3]), (PSZ)(VarsCmd.FileHandleTable[i] + 1)) == 0)
+ {
+ *(ArgV[2]) = (VarsCmd.FileHandleTable[i][0] == 'w');
+ break;
+ }
+ }
+
+ if (i == MAX_HANDLES)
+ {
+ i = NOT_A_HANDLE;
+ *((UWORD *)ArgV[0]) = HANDLEALREADYCLOSED;
+ }
+ else
+ {
+ *((UWORD *)ArgV[0]) = SUCCESS;
+ }
+
+ *(ArgV[1]) = i;
+
+ return NO_ERR;
+}
+
+
+//cCmdWrapFileRename
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: Old Filename, CStr
+//ArgV[2]: New Filename, CStr
+NXT_STATUS cCmdWrapFileRename (UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ ULONG Tmp;
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ //!!! Tmp placeholder passed into loader code to avoid illegal dereferencing.
+ LStatus = pMapLoader->pFunc(RENAMEFILE, ArgV[1], ArgV[2], &Tmp);
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+
+ return NO_ERR;
+}
+
+
+//cCmdWrapFileDelete
+//ArgV[0]: (Function return) Loader status, U16 return
+//ArgV[1]: Filename, CStr
+NXT_STATUS cCmdWrapFileDelete (UBYTE * ArgV[])
+{
+ LOADER_STATUS LStatus;
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+
+ LStatus = pMapLoader->pFunc(DELETE, ArgV[1], NULL, NULL);
+
+ //Status code in high byte of LStatus
+ *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
+
+ return NO_ERR;
+}
+
+//
+//cCmdWrapSoundPlayFile
+//ArgV[0]: (Return value) Status code, SBYTE
+//ArgV[1]: Filename, CStr
+//ArgV[2]: Loop?, UBYTE (bool)
+//ArgV[3]: Volume, UBYTE
+//
+NXT_STATUS cCmdWrapSoundPlayFile(UBYTE * ArgV[])
+{
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ UBYTE sndVol= *(ArgV[3]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+
+ //!!! Should check filename and/or existence and return error before proceeding
+ strncpy((PSZ)(pMapSound->SoundFilename), (PSZ)(ArgV[1]), FILENAME_LENGTH);
+
+ if (*(ArgV[2]) == TRUE)
+ pMapSound->Mode = SOUND_LOOP;
+ else
+ pMapSound->Mode = SOUND_ONCE;
+
+ if(sndVol > 4)
+ sndVol= 4;
+ pMapSound->Volume = sndVol;
+ //SampleRate of '0' means "let file specify SampleRate"
+ pMapSound->SampleRate = 0;
+ pMapSound->Flags |= SOUND_UPDATE;
+
+ *((SBYTE*)(ArgV[0])) = (NO_ERR);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapSoundPlayTone
+//ArgV[0]: (Return value) Status code, SBYTE
+//ArgV[1]: Frequency, UWORD
+//ArgV[2]: Duration, UWORD
+//ArgV[3]: Loop?, UBYTE (Boolean)
+//ArgV[4]: Volume, UBYTE
+//
+NXT_STATUS cCmdWrapSoundPlayTone(UBYTE * ArgV[])
+{
+ UBYTE sndVol= *(ArgV[4]);
+ pMapSound->Freq = *(UWORD*)(ArgV[1]);
+ pMapSound->Duration = *(UWORD*)(ArgV[2]);
+ if(sndVol > 4)
+ sndVol= 4;
+ pMapSound->Volume = sndVol;
+ pMapSound->Flags |= SOUND_UPDATE;
+
+ if (*(ArgV[3]) == TRUE)
+ pMapSound->Mode = SOUND_TONE | SOUND_LOOP;
+ else
+ pMapSound->Mode = SOUND_TONE;
+
+ *((SBYTE*)(ArgV[0])) = (NO_ERR);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapSoundGetState
+//ArgV[0]: (Return value) sound module state, UBYTE
+//ArgV[1]: Flags, UBYTE
+//
+NXT_STATUS cCmdWrapSoundGetState(UBYTE * ArgV[])
+{
+ *(ArgV[0]) = pMapSound->State;
+ *(ArgV[1]) = pMapSound->Flags;
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapSoundSetState
+//ArgV[0]: (Return value) sound module state, UBYTE
+//ArgV[1]: State, UBYTE
+//ArgV[2]: Flags, UBYTE
+//
+NXT_STATUS cCmdWrapSoundSetState(UBYTE * ArgV[])
+{
+ pMapSound->State = *(ArgV[1]);
+ //Return same state we just set, mostly for interface consistency
+ *(ArgV[0]) = pMapSound->State;
+
+ //OR in provided flags (usually 0)
+ pMapSound->Flags |= *(ArgV[2]);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapReadButton
+//ArgV[0]: (Function return) Status code, SBYTE
+//ArgV[1]: Index (U8)
+//ArgV[2]: Pressed (bool)
+//ArgV[3]: Count (U8) (count of press-then-release cycles)
+//ArgV[4]: ResetCount? (bool in)
+//
+NXT_STATUS cCmdWrapReadButton(UBYTE * ArgV[])
+{
+ UBYTE btnIndex;
+
+ btnIndex = *((UBYTE*)(ArgV[1]));
+
+ if (btnIndex < NO_OF_BTNS)
+ {
+ //Set pressed boolean output
+ if (pMapButton->State[btnIndex] & PRESSED_STATE)
+ *(ArgV[2]) = TRUE;
+ else
+ *(ArgV[2]) = FALSE;
+
+ //Set count output
+ *(ArgV[3]) = (UBYTE)(pMapButton->BtnCnt[btnIndex].RelCnt);
+
+ //Optionally reset internal count
+ if (*(ArgV[4]) != 0)
+ {
+ pMapButton->BtnCnt[btnIndex].RelCnt = 0;
+ //Need to clear short and long counts too, because RelCnt depends on them. No known side effects.
+ pMapButton->BtnCnt[btnIndex].ShortRelCnt = 0;
+ pMapButton->BtnCnt[btnIndex].LongRelCnt = 0;
+ }
+
+ // Set status code 'OK'
+ *((SBYTE*)(ArgV[0])) = NO_ERR;
+ }
+ else
+ {
+ //Bad button index specified, return error and default outputs
+ *((SBYTE*)(ArgV[0])) = ERR_INVALID_PORT;
+ *(ArgV[2]) = FALSE;
+ *(ArgV[3]) = 0;
+ }
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommLSWrite
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used
+//ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device
+//
+NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE Port = *(ArgV[1]);
+ UBYTE * pBuf;
+ UWORD BufLength;
+ UBYTE ResponseLength = *(ArgV[3]);
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ pBuf = cCmdDVPtr(DVIndex);
+ BufLength = DV_ARRAY[DVIndex].Count;
+
+ *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommLSCheckStatus
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: BytesReady, UBYTE
+//
+NXT_STATUS cCmdWrapCommLSCheckStatus(UBYTE * ArgV[])
+{
+ UBYTE Port = *(ArgV[1]);
+
+ *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port);
+ *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommLSRead
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: Buffer for data, UBYTE array, max SIZE_OF_LSBUF bytes will be written
+//ArgV[3]: BufferLength, UBYTE, specifies size of buffer requested
+//
+NXT_STATUS cCmdWrapCommLSRead(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE Port = *(ArgV[1]);
+ UBYTE * pBuf;
+ UBYTE BufLength = *(ArgV[3]);
+ UBYTE BytesToRead;
+ DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[2]);
+ NXT_STATUS AllocStatus;
+
+ *pReturnVal = cCmdLSCheckStatus(Port);
+ BytesToRead = cCmdLSCalcBytesReady(Port);
+
+ //If channel is OK and has data ready for us, put the data into outgoing buffer
+ if (!IS_ERR(*pReturnVal) && BytesToRead > 0)
+ {
+ //Limit buffer to available data
+ if (BufLength > BytesToRead)
+ BufLength = BytesToRead;
+
+ AllocStatus = cCmdDVArrayAlloc(DVIndex, BufLength);
+ if (IS_ERR(AllocStatus))
+ return (AllocStatus);
+
+ pBuf = cCmdDVPtr(DVIndex);
+ *pReturnVal = cCmdLSRead(Port, BufLength, pBuf);
+ }
+ //Else, the channel has an error and/or there's no data to read; clear the output array
+ else
+ {
+ AllocStatus = cCmdDVArrayAlloc(DVIndex, 0);
+ if (IS_ERR(AllocStatus))
+ return (AllocStatus);
+ }
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapRandomNumber
+//ArgV[0]: (return) Random number, SWORD
+//
+NXT_STATUS cCmdWrapRandomNumber(UBYTE * ArgV[])
+{
+ static UBYTE count = 0;
+ SWORD random;
+
+ if (count == 0)
+ srand(dTimerRead());
+
+ if (count > 20)
+ count = 0;
+ else
+ count++;
+
+ //!!! IAR's implementation of the rand() library function returns signed values, and we want it that way.
+ //Some stdlib implementations may return only positive numbers, so be wary if this code is ported.
+ random = rand();
+
+ *((SWORD *)ArgV[0]) = random;
+
+ return NO_ERR;
+}
+
+//
+//cCmdWrapGetStartTick
+//ArgV[0]: (return) Start Tick, ULONG
+//
+NXT_STATUS cCmdWrapGetStartTick(UBYTE * ArgV[])
+{
+ *((ULONG *)ArgV[0]) = VarsCmd.StartTick;
+ return NO_ERR;
+}
+
+//
+//cCmdWrapMessageWrite
+//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
+//ArgV[1]: QueueID, UBYTE
+//ArgV[2]: Message, CStr
+//
+NXT_STATUS cCmdWrapMessageWrite(UBYTE * ArgV[])
+{
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ ArgV[2] = cCmdDVPtr(DVIndex);
+
+ Status = cCmdMessageWrite(*(UBYTE *)(ArgV[1]), ArgV[2], DV_ARRAY[DVIndex].Count);
+
+ *(SBYTE *)(ArgV[0]) = Status;
+
+ if (IS_FATAL(Status))
+ return Status;
+ else
+ return (NO_ERR);
+}
+
+
+
+//
+//cCmdWrapColorSensorRead
+//ArgV[0]: (return) Error code, SBYTE
+//ArgV[1]: Port, UBYTE
+//ArgV[2]: SensorValue, SWORD
+//ArgV[3]: RawArray, UWORD[NO_OF_COLORS]
+//ArgV[4]: NormalizedArray, UWORD[NO_OF_COLORS]
+//ArgV[5]: ScaledArray, SWORD[NO_OF_COLORS]
+//ArgV[6]: InvalidData, UBYTE
+//
+NXT_STATUS cCmdWrapColorSensorRead (UBYTE * ArgV[])
+{
+ DV_INDEX DVIndex;
+ NXT_STATUS Status = NO_ERR;
+ //Resolve return val arguments
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ //Resolve Port argument
+ UBYTE Port = *(UBYTE*)(ArgV[1]);
+ //Resolve SensorValue
+ SWORD SensorValue = *(SWORD*)(ArgV[2]);
+ //Resolve RawArray as array
+ DVIndex = *(DV_INDEX*)(ArgV[3]);
+ NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
+ Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
+ if (IS_ERR(Status))
+ return (Status);
+ ArgV[3] = cCmdDVPtr (DVIndex);
+ //Resolve NormalizedArray as array
+ DVIndex = *(DV_INDEX*)(ArgV[4]);
+ NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
+ Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
+ if (IS_ERR(Status))
+ return (Status);
+ ArgV[4] = cCmdDVPtr (DVIndex);
+ //Resolve ScaledArray as array
+ DVIndex = *(DV_INDEX*)(ArgV[5]);
+ NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
+ Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
+ if (IS_ERR(Status))
+ return (Status);
+ ArgV[5] = cCmdDVPtr (DVIndex);
+ //Resolve InvalidData
+ UBYTE InvalidData = *(UBYTE*)(ArgV[6]);
+
+ //call implementation with unwrapped parameters
+ *pReturnVal = cCmdColorSensorRead (Port, &SensorValue, (UWORD*)ArgV[3], (UWORD*)ArgV[4], (SWORD*)ArgV[5], &InvalidData);
+
+ *(ArgV[2]) = SensorValue;
+ *(ArgV[6]) = InvalidData;
+
+ if (IS_ERR(*pReturnVal)){
+ return (*pReturnVal);
+ }
+ return NO_ERR;
+}
+
+
+#define UNPACK_STATUS(StatusWord) ((SBYTE)(StatusWord))
+
+NXT_STATUS cCmdBTCheckStatus(UBYTE Connection)
+{
+ //If specified connection is invalid, return error code to the user.
+ if (Connection >= SIZE_OF_BT_CONNECT_TABLE)
+ {
+ return (ERR_INVALID_PORT);
+ }
+
+ //INPROGRESS means a request is currently pending completion by the comm module
+ if (VarsCmd.CommStat == INPROGRESS)
+ {
+ return (STAT_COMM_PENDING);
+ }
+ //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY
+ //And check if specified connection is indeed configured
+ else if (VarsCmd.CommStat == (SWORD)BTBUSY
+ || (pMapComm->BtConnectTable[Connection].Name[0]) == '\0')
+ {
+ return (ERR_COMM_CHAN_NOT_READY);
+ }
+ else
+ {
+ return (UNPACK_STATUS(VarsCmd.CommStat));
+ }
+}
+
+//Default packet to send for a remote MESSAGE_READ command.
+//3rd byte must be replaced with remote mailbox (QueueID)
+//4th byte must be replaced with local mailbox
+static UBYTE RemoteMsgReadPacket[5] = {0x00, 0x13, 0xFF, 0xFF, 0x01};
+
+//
+//cCmdWrapMessageRead
+//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
+//ArgV[1]: QueueID, UBYTE
+//ArgV[2]: Remove, UBYTE
+//ArgV[3]: (return) Message, CStr
+//
+NXT_STATUS cCmdWrapMessageRead(UBYTE * ArgV[])
+{
+ NXT_STATUS Status = NO_ERR;
+ NXT_STATUS AllocStatus = NO_ERR;
+ UBYTE QueueID = *(UBYTE *)(ArgV[1]);
+ DV_INDEX DestDVIndex = *(DV_INDEX *)(ArgV[3]);
+ UWORD MessageSize;
+ UBYTE i;
+
+ NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
+
+ //Check Next Message's size
+ Status = cCmdMessageGetSize(QueueID, &MessageSize);
+
+ //If there is a valid message in local mailbox, read it
+ if (!IS_ERR(Status) && MessageSize > 0 )
+ {
+ //!!! Also check for EMPTY_MAILBOX status?
+ //Size destination string
+ AllocStatus = cCmdDVArrayAlloc(DestDVIndex, MessageSize);
+ if (IS_ERR(AllocStatus))
+ return AllocStatus;
+
+ //Get Message
+ //!!! Should more aggressively enforce null termination before blindly copying to dataspace
+ Status = cCmdMessageRead(QueueID, cCmdDVPtr(DestDVIndex), MessageSize, *(ArgV[2]));
+ }
+ else
+ {
+ //Clear destination string
+ AllocStatus = cCmdDVArrayAlloc(DestDVIndex, 1);
+ if (IS_ERR(AllocStatus))
+ return AllocStatus;
+
+ //Successful allocation, make sure first byte is null terminator
+ *(UBYTE*)(cCmdDVPtr(DestDVIndex)) = '\0';
+ }
+
+ //If there were no local messages, see if there are any waiting in our slaves' outboxes
+ if (Status == STAT_MSG_EMPTY_MAILBOX && QueueID < INCOMING_QUEUE_COUNT)
+ {
+ //If there's an old error code hanging around, clear it before proceeding.
+ //!!! Clearing error here means bytecode status checking loops could get false SUCCESS results?
+ if (VarsCmd.CommStat < 0)
+ VarsCmd.CommStat = SUCCESS;
+
+ //Search through possible slaves, looking for valid connection
+ for (i = 0; i < SIZE_OF_BT_CONNECT_TABLE - 1; i++)
+ {
+ //Advance CommCurrConnection and limit to 1, 2, or 3 (only slave connection slots are checked)
+ VarsCmd.CommCurrConnection++;
+ if (VarsCmd.CommCurrConnection == SIZE_OF_BT_CONNECT_TABLE)
+ VarsCmd.CommCurrConnection = 1;
+
+ if (cCmdBTCheckStatus(VarsCmd.CommCurrConnection) == NO_ERR)
+ break;
+ }
+
+ //If there is at least one configured slave connection, make a remote read request
+ if (i < SIZE_OF_BT_CONNECT_TABLE - 1)
+ {
+ //Outgoing QueueID on slave device is the local QueueID + INCOMING_QUEUE_COUNT
+ RemoteMsgReadPacket[2] = QueueID + INCOMING_QUEUE_COUNT;
+ RemoteMsgReadPacket[3] = QueueID;
+
+ //Request comm module to send assembled packet and not go idle until response comes back (or error)
+ pMapComm->pFunc(SENDDATA, sizeof(RemoteMsgReadPacket), VarsCmd.CommCurrConnection, TRUE, RemoteMsgReadPacket, (UWORD*)&(VarsCmd.CommStat));
+
+ //Read status back after SENDDATA call so bytecode gets STAT_COMM_PENDING or error
+ Status = cCmdBTCheckStatus(VarsCmd.CommCurrConnection);
+
+ //If our request was accepted, set the DirtyComm flag so stream will get cleaned up later
+ if (Status == STAT_COMM_PENDING)
+ VarsCmd.DirtyComm = TRUE;
+ }
+ }
+
+ *(SBYTE *)(ArgV[0]) = Status;
+ if (IS_FATAL(Status))
+ return Status;
+ else
+ return (NO_ERR);
+}
+
+
+//
+//cCmdWrapCommBTCheckStatus
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Connection index, 0-3
+//
+NXT_STATUS cCmdWrapCommBTCheckStatus(UBYTE * ArgV[])
+{
+ *((SBYTE*)(ArgV[0])) = cCmdBTCheckStatus(*(ArgV[1]));
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommBTWrite
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Connection index, 0-3
+//ArgV[2]: Buffer
+//
+NXT_STATUS cCmdWrapCommBTWrite(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE Connection = *(ArgV[1]);
+ UBYTE * pBuf;
+ UWORD BufLength;
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ pBuf = cCmdDVPtr(DVIndex);
+
+ BufLength = DV_ARRAY[DVIndex].Count;
+
+ //If there's an old error code hanging around, clear it before proceeding.
+ if (VarsCmd.CommStat < 0)
+ VarsCmd.CommStat = SUCCESS;
+
+ //!!! Only first 256 bytes could possibly make it through! Should return error on longer input?
+ //!!! Not requesting a wait-for-response because only known use doesn't read responses.
+ pMapComm->pFunc(SENDDATA, (UBYTE)BufLength, Connection, FALSE, pBuf, (UWORD*)&(VarsCmd.CommStat));
+
+ //!!! Reasonable to wrap below code in cCmdCommBTCheckStatus?
+ //INPROGRESS means our request was accepted by His Funkiness of pFunc
+ if (VarsCmd.CommStat == (SWORD)INPROGRESS)
+ {
+ *pReturnVal = STAT_COMM_PENDING;
+
+ //Set DirtyComm flag so stream is reset after program ends
+ VarsCmd.DirtyComm = TRUE;
+ }
+ //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY
+ else if (VarsCmd.CommStat == (SWORD)BTBUSY)
+ {
+ *pReturnVal = ERR_COMM_CHAN_NOT_READY;
+ }
+ else
+ {
+ *pReturnVal = UNPACK_STATUS(VarsCmd.CommStat);
+ }
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommBTRead
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Count to read
+//ArgV[2]: Buffer
+//
+NXT_STATUS cCmdWrapCommBTRead(UBYTE * ArgV[])
+{
+ //SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ //UBYTE * pBuf = (ArgV[2]);
+ //!!! should provide length and/or connection to read?
+
+ //!!! This syscall is not implemented; return fatal error.
+ return (ERR_INSTR);
+}
+
+//
+//cCmdWrapKeepAlive
+//ArgV[0]: (return) Current timer limit in ms, ULONG
+//
+NXT_STATUS cCmdWrapKeepAlive(UBYTE * ArgV[])
+{
+ pMapUi->Flags |= UI_RESET_SLEEP_TIMER;
+
+ //Convert UI's minute-based timeout value to millisecs
+ //Milliseconds are the "natural" time unit in user-land.
+ *(ULONG*)(ArgV[0]) = (pMapUi->SleepTimeout * 60 * 1000);
+
+ return (NO_ERR);
+}
+
+
+
+#define MAX_IOM_BUFFER_SIZE 64
+//
+//cCmdWrapIOMapRead
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Module name, CStr
+//ArgV[2]: Offset, UWORD
+//ArgV[3]: Count, UWORD
+//ArgV[4]: Buffer, UBYTE array
+//
+NXT_STATUS cCmdWrapIOMapRead(UBYTE * ArgV[])
+{
+ UWORD LStatus;
+ NXT_STATUS Status;
+
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UWORD Offset = *(UWORD*)(ArgV[2]);
+ //Our copy of 'Count' must be a ULONG to match the loader interface
+ ULONG Count = *(UWORD*)(ArgV[3]);
+
+ DV_INDEX DVIndex;
+
+ //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc
+ //We need it to transfer the ModuleID to the IOMAPREAD call
+ UBYTE FindBuffer[FILENAME_LENGTH + 10];
+ //Buffer to store data and offset in for IOMAPREAD call
+ //!!! Constant size means only limited reads and writes
+ UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2];
+
+ if (Count > MAX_IOM_BUFFER_SIZE)
+ {
+ //Request to read too much data at once; clear buffer, return error.
+ DVIndex = *(DV_INDEX *)(ArgV[4]);
+ *pReturnVal = cCmdDVArrayAlloc(DVIndex, 0);
+ if (IS_ERR(*pReturnVal))
+ return (*pReturnVal);
+
+ *pReturnVal = ERR_INVALID_SIZE;
+ return (NO_ERR);
+ }
+
+ //Resolve module name
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+
+ //Find module by name. Note that wildcards are accepted, but only first match matters.
+ LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL);
+
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ //Module was found, transfer Offset into first two bytes of DataBuffer and attempt to read
+ *(UWORD*)(DataBuffer) = Offset;
+ LStatus = pMapLoader->pFunc(IOMAPREAD, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count);
+
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ //No error from IOMAPREAD, so copy the data into VM's dataspace
+ //Size destination array
+ DVIndex = *(DV_INDEX *)(ArgV[4]);
+ Status = cCmdDVArrayAlloc(DVIndex, (UWORD)Count);
+ if (IS_ERR(Status))
+ {
+ //Alloc failed, so close handle and return
+ pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
+ return (Status);
+ }
+
+ //Alloc succeeded, so resolve and copy away
+ ArgV[4] = cCmdDVPtr(DVIndex);
+ memcpy(ArgV[4], &(DataBuffer[2]), Count);
+ }
+ }
+
+ *pReturnVal = LOADER_ERR_BYTE(LStatus);
+
+ pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapIOMapWrite
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Module name, CStr
+//ArgV[2]: Offset, UWORD
+//ArgV[3]: Buffer, UBYTE array
+//
+NXT_STATUS cCmdWrapIOMapWrite(UBYTE * ArgV[])
+{
+ UWORD LStatus;
+
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UWORD Offset = *(UWORD*)(ArgV[2]);
+
+ //Our copy of 'Count' must be a ULONG to match the loader interface
+ ULONG Count;
+ DV_INDEX DVIndex;
+
+ //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc
+ //We need it to transfer the ModuleID to the IOMAPREAD call
+ UBYTE FindBuffer[FILENAME_LENGTH + 10];
+ //Buffer to store data and offset in for IOMAPREAD call
+ //!!! Constant size means only limited reads and writes
+ UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2];
+
+ //Resolve module name and buffer
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+
+ DVIndex = *(DV_INDEX *)(ArgV[3]);
+ ArgV[3] = cCmdDVPtr(DVIndex);
+ Count = DV_ARRAY[DVIndex].Count;
+
+ if (Count > MAX_IOM_BUFFER_SIZE)
+ {
+ //Request to read too much data at once; return error and give up
+ *pReturnVal = ERR_INVALID_SIZE;
+ return (NO_ERR);
+ }
+
+ LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL);
+
+ if (LOADER_ERR(LStatus) == SUCCESS)
+ {
+ //Module was found, transfer Offset into first two bytes of DataBuffer, copy data into rest of buffer, then write
+ *(UWORD*)(DataBuffer) = Offset;
+ memcpy(&(DataBuffer[2]), ArgV[3], Count);
+ LStatus = pMapLoader->pFunc(IOMAPWRITE, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count);
+ }
+
+ *pReturnVal = LOADER_ERR_BYTE(LStatus);
+
+ pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
+ return (NO_ERR);
+}
+
+#if VM_BENCHMARK
+void cCmdWriteBenchmarkFile()
+{
+ LOADER_STATUS LStatus;
+ UBYTE Handle;
+ ULONG BenchFileSize;
+ ULONG i, Length;
+ UBYTE Buffer[256];
+
+ //Remove old benchmark file, create a new one
+ strcpy((char *)Buffer, "benchmark.txt");
+ pMapLoader->pFunc(DELETE, Buffer, NULL, NULL);
+ BenchFileSize = 2048;
+ LStatus = pMapLoader->pFunc(OPENWRITEDATA, Buffer, NULL, &BenchFileSize);
+
+ if (!LOADER_ERR(LStatus))
+ {
+ //Write Benchmark file
+ Handle = LOADER_HANDLE(LStatus);
+
+ //Header
+ sprintf((char *)Buffer, "Program Name: %s\r\n", VarsCmd.ActiveProgName);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "InstrCount: %d\r\n", VarsCmd.InstrCount);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "Time: %d\r\n", IOMapCmd.Tick - VarsCmd.StartTick);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "Instr/Tick: %d\r\n", VarsCmd.Average);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "CmdCtrl Calls: %d\r\n", VarsCmd.CmdCtrlCount);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "OverTime Rounds: %d\r\n", VarsCmd.OverTimeCount);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "Max OverTime Length: %d\r\n", VarsCmd.MaxOverTimeLength);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "CompactionCount: %d\r\n", VarsCmd.CompactionCount);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "LastCompactionTick: %d\r\n", VarsCmd.LastCompactionTick);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "MaxCompactionTime: %d\r\n", VarsCmd.MaxCompactionTime);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ //opcode benchmarks
+ sprintf((char *)Buffer, "Op\tCnt\tOver\tMax\r\n");
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+ for (i = 0; i < OPCODE_COUNT; i++)
+ {
+ sprintf((char *)Buffer, "%x\t%d\t%d\t%d\t%d\r\n", i, VarsCmd.OpcodeBenchmarks[i][0], VarsCmd.OpcodeBenchmarks[i][1], VarsCmd.OpcodeBenchmarks[i][2], VarsCmd.OpcodeBenchmarks[i][3]);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+ }
+ //close file
+ LStatus = pMapLoader->pFunc(CLOSE, &Handle, NULL, NULL);
+ }
+}
+#endif
+
+
+/////////////////////////////////////////////////////////////
+// Dymanic syscall implementations
+////////////////////////////////////////////////////////////
+
+//
+//cCmdWrapDatalogWrite
+//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
+//ArgV[1]: Message, CStr
+//
+NXT_STATUS cCmdWrapDatalogWrite(UBYTE * ArgV[])
+{
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[1]);
+ ArgV[1] = cCmdDVPtr(DVIndex);
+
+ Status = cCmdDatalogWrite(ArgV[1], DV_ARRAY[DVIndex].Count);
+
+ *(SBYTE *)(ArgV[0]) = Status;
+
+ if (IS_FATAL(Status))
+ return Status;
+ else
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapDatalogGetTimes
+//ArgV[0]: SyncTime, U32
+//ArgV[1]: SyncTick, U32
+//
+NXT_STATUS cCmdWrapDatalogGetTimes(UBYTE * ArgV[])
+{
+ *((ULONG *)ArgV[1]) = IOMapCmd.SyncTime;
+ *((ULONG *)ArgV[2]) = IOMapCmd.SyncTick;
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapSetSleepTimeout
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: desired timer limit in ms, ULONG
+//
+NXT_STATUS cCmdWrapSetSleepTimeout(UBYTE * ArgV[])
+{
+ ULONG value = *(ULONG*)(ArgV[1]);
+ if(value==0)
+ {
+ pMapUi->SleepTimeout=0;
+ }
+ else if(value < 60000)
+ {
+ pMapUi->SleepTimeout=1; //integer math would've made this zero
+ }
+ else
+ {
+ pMapUi->SleepTimeout= value / 60000;
+ }
+ return (NO_ERR);
+}
+
+// currently copied from LS, not finished.
+//
+//cCmdWrapCommHSWrite
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used
+//ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device
+//
+NXT_STATUS cCmdWrapCommHSWrite(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE Port = *(ArgV[1]);
+ UBYTE * pBuf;
+ UWORD BufLength;
+ UBYTE ResponseLength = *(ArgV[3]);
+ DV_INDEX DVIndex;
+
+ //Resolve array arguments
+ DVIndex = *(DV_INDEX *)(ArgV[2]);
+ pBuf = cCmdDVPtr(DVIndex);
+ BufLength = DV_ARRAY[DVIndex].Count;
+
+ *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommHSCheckStatus
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: BytesReady, UBYTE
+//
+NXT_STATUS cCmdWrapCommHSCheckStatus(UBYTE * ArgV[])
+{
+ UBYTE Port = *(ArgV[1]);
+
+ *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port);
+ *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port);
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommHSRead
+//ArgV[0]: (return) Status code, SBYTE
+//ArgV[1]: Port specifier, UBYTE
+//ArgV[2]: Buffer for data, UBYTE array, max SIZE_OF_LSBUF bytes will be written
+//ArgV[3]: BufferLength, UBYTE, specifies size of buffer requested
+//
+NXT_STATUS cCmdWrapCommHSRead(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE Port = *(ArgV[1]);
+ UBYTE * pBuf;
+ UBYTE BufLength = *(ArgV[3]);
+ UBYTE BytesToRead;
+ DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[2]);
+ NXT_STATUS AllocStatus;
+
+ *pReturnVal = cCmdLSCheckStatus(Port);
+ BytesToRead = cCmdLSCalcBytesReady(Port);
+
+ //If channel is OK and has data ready for us, put the data into outgoing buffer
+ if (!IS_ERR(*pReturnVal) && BytesToRead > 0)
+ {
+ //Limit buffer to available data
+ if (BufLength > BytesToRead)
+ BufLength = BytesToRead;
+
+ AllocStatus = cCmdDVArrayAlloc(DVIndex, BufLength);
+ if (IS_ERR(AllocStatus))
+ return (AllocStatus);
+
+ pBuf = cCmdDVPtr(DVIndex);
+ *pReturnVal = cCmdLSRead(Port, BufLength, pBuf);
+ }
+ //Else, the channel has an error and/or there's no data to read; clear the output array
+ else
+ {
+ AllocStatus = cCmdDVArrayAlloc(DVIndex, 0);
+ if (IS_ERR(AllocStatus))
+ return (AllocStatus);
+ }
+
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommBTOnOff
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Power State, 0-1
+//
+NXT_STATUS cCmdWrapCommBTOnOff(UBYTE * ArgV[])
+{
+ UWORD retVal;
+ NXT_STATUS status;
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+
+ UBYTE powerState = *(ArgV[1]);
+ if(powerState)
+ status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
+ else
+ status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);
+
+ *pReturnVal= (status == SUCCESS) ? retVal : status;
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapCommBTConnection
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Action, UBYTE
+//ArgV[2]: name, UBYTE array CStr
+//ArgV[3]: connection slot, UBYTE
+//
+NXT_STATUS cCmdWrapCommBTConnection(UBYTE * ArgV[])
+{
+ UWORD retVal;
+ NXT_STATUS status;
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UBYTE *nmPtr;
+
+ UBYTE action = *(ArgV[1]);
+ UBYTE connection = *(ArgV[3]);
+ nmPtr = cCmdDVPtr(*(DV_INDEX *)(ArgV[2]));
+
+ if(action) // Init
+ status= pMapComm->pFunc(CONNECTBYNAME, 0, connection, 0, nmPtr, &retVal);
+ else // Close
+ status= pMapComm->pFunc(DISCONNECT, connection, 0, 0, NULL, &retVal);
+
+ *pReturnVal= (status == SUCCESS) ? retVal : status;
+ return (NO_ERR);
+}
+
+
+//
+//cCmdWrapReadSemData
+//ArgV[0]: return data, U8
+//ArgV[1]: which (0=used, 1=request), U8
+//
+NXT_STATUS cCmdWrapReadSemData(UBYTE * ArgV[])
+{
+ if(!(*((UBYTE *)ArgV[1])))
+ *((UBYTE *)ArgV[0])= gUsageSemData;
+ else
+ *((UBYTE *)ArgV[0])= gRequestSemData;
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapWriteSemData
+//ArgV[0]: return data, U8
+//ArgV[1]: which (0=used, 1=request), U8
+//ArgV[2]: newValue, U8
+//ArgV[3]: action (0= OR, 1= AND), U8
+//
+NXT_STATUS cCmdWrapWriteSemData(UBYTE * ArgV[])
+{
+ UBYTE curVal, newVal, which= (*((UBYTE *)ArgV[1]));
+ if(!which)
+ curVal= gUsageSemData;
+ else
+ curVal= gRequestSemData;
+
+ newVal= *((UBYTE *)ArgV[2]);
+
+ if(*((UBYTE *)ArgV[3]))
+ curVal &= ~newVal;
+ else
+ curVal |= newVal;
+
+ if(!which)
+ gUsageSemData= curVal;
+ else
+ gRequestSemData= curVal;
+ *((UBYTE *)ArgV[0])= curVal;
+ return (NO_ERR);
+}
+
+
+//
+//cCmdWrapUpdateCalibCacheInfo
+//ArgV[0]: return data, U8
+//ArgV[1]: nm, UBYTE array CStr
+//ArgV[2]: min, U16
+//ArgV[3]: max , U16
+//
+NXT_STATUS cCmdWrapUpdateCalibCacheInfo(UBYTE * ArgV[])
+{
+ UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
+ SWORD min= (*((SWORD *)ArgV[2]));
+ SWORD max= (*((SWORD *)ArgV[3]));
+
+ cCmdUpdateCalibrationCache(nm, min, max);
+ *((UBYTE *)ArgV[0])= SUCCESS;
+ return (NO_ERR);
+}
+
+//
+//cCmdWrapComputeCalibValue
+//ArgV[0]: return data, U8
+//ArgV[1]: nm, UBYTE array CStr
+//ArgV[2]: raw, U16 ref in out
+NXT_STATUS cCmdWrapComputeCalibValue (UBYTE * ArgV[])
+{
+ UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
+ SWORD raw= (*((SWORD *)ArgV[2]));
+
+ *((UBYTE *)ArgV[0])= cCmdComputeCalibratedValue(nm, &raw);
+ (*((SWORD *)ArgV[2]))= raw;
+ return (NO_ERR);
+}
+
+typedef struct {
+ SWORD min, max;
+ UBYTE nm[FILENAME_LENGTH + 1];
+} CalibCacheType;
+
+SBYTE gCalibCacheCnt= 0;
+DV_INDEX gCalibCacheArrayDVIdx= NOT_A_DS_ID;
+CalibCacheType *gCalibCacheArray= NULL;
+
+SWORD cCmdGetCalibrationIndex(UBYTE *nm) {
+ SBYTE i;
+ for(i= 0; i < gCalibCacheCnt; i++)
+ if(!strcmp((PSZ)nm, (PSZ)gCalibCacheArray[i].nm))
+ break;
+ return i;
+}
+
+NXT_STATUS cCmdComputeCalibratedValue(UBYTE *nm, SWORD *pRaw) {
+ SBYTE i= cCmdGetCalibrationIndex(nm);
+ NXT_STATUS status= ERR_RC_ILLEGAL_VAL;
+ SLONG raw= *pRaw, range;
+ if(i < gCalibCacheCnt) {
+ status= SUCCESS;
+ raw -= gCalibCacheArray[i].min;
+ range= (gCalibCacheArray[i].max - gCalibCacheArray[i].min);
+ }
+ else
+ range= 1023;
+ raw *= 100;
+ raw /= range;
+ if(raw < 0) raw= 0;
+ else if(raw > 100) raw= 100;
+ *pRaw= raw;
+ return status;
+}
+
+
+NXT_STATUS ResizeCalibCache(ULONG elements) { // alloc dv if needed, grow if needed. dv never freed. on boot, set to NOT_A_DS_ID. use cnt for valid elements.
+ NXT_STATUS Status = NO_ERR;
+
+ if(gCalibCacheArrayDVIdx == NOT_A_DS_ID)
+ Status = cCmdAllocDopeVector(&gCalibCacheArrayDVIdx, sizeof(CalibCacheType));
+ if(!IS_ERR(Status) && DV_ARRAY[gCalibCacheArrayDVIdx].Count < elements) //Allocate storage for cache element
+ Status = cCmdDVArrayAlloc(gCalibCacheArrayDVIdx, elements);
+ if(!IS_ERR(Status))
+ gCalibCacheArray= cCmdDVPtr(gCalibCacheArrayDVIdx);
+ // on error, does old DVIdx still point to array, or should we null out array???
+ return Status;
+}
+
+// called to update min/max on existing cache element, and to add new named element
+void cCmdUpdateCalibrationCache(UBYTE *nm, SWORD min, SWORD max) {
+ SWORD i= cCmdGetCalibrationIndex(nm);
+ NXT_STATUS Status = NO_ERR;
+
+ if(i == gCalibCacheCnt) { // sensor wasn't found, insert into cache
+ Status= ResizeCalibCache(gCalibCacheCnt+1);
+ if(!IS_ERR(Status)) {
+ gCalibCacheCnt++;
+ strcpy((PSZ)gCalibCacheArray[i].nm, (PSZ)nm);
+ }
+ }
+ if(!IS_ERR(Status)) {
+ gCalibCacheArray[i].min= min;
+ gCalibCacheArray[i].max= max;
+ }
+}
+
+void cCmdLoadCalibrationFiles(void) {
+ ULONG cnt, DataSize;
+ UBYTE nm[FILENAME_LENGTH + 1], nmLen;
+ SWORD Handle, HandleSearch;
+ gCalibCacheCnt= 0;
+ gCalibCacheArrayDVIdx= NOT_A_DS_ID;
+ // file I/O to load all .cal files into cached globals used by scaling syscall
+ HandleSearch = pMapLoader->pFunc(FINDFIRST, "*.cal", nm, &cnt); // returns total files and nm of first one
+ while (LOADER_ERR(HandleSearch) == SUCCESS) { // if we have a file, process it by closing and opening
+ SWORD min= 0, max= 0, tmp;
+ ULONG length;
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
+ Handle = pMapLoader->pFunc(OPENREAD, nm, NULL, &DataSize);
+ if (LOADER_ERR(Handle) == SUCCESS && DataSize == 4) {
+ // access data, two bytes for min and two for max
+ length= 2;
+ pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
+ if (length == 2)
+ min= tmp;
+ length= 2;
+ pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
+ if (length == 2)
+ max= tmp;
+ }
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(Handle), NULL, NULL);
+ // update calibration cache with nm, min, and max
+ nmLen= strlen((PSZ)nm) - 4; // chop off .cal extension
+ nm[nmLen]= 0;
+ cCmdUpdateCalibrationCache(nm, min, max);
+
+ HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), nm, &cnt);
+ }
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
+}
+
+//
+//cCmdWrapListFiles
+//ArgV[0]: return data, SBYTE
+//ArgV[1]: pattern, UBYTE array CStr
+//ArgV[2]: list, UBYTE array CStr array ref in out
+NXT_STATUS cCmdWrapListFiles (UBYTE * ArgV[])
+{
+ ULONG fileSize, matchCount=0, i=0, oldCount;
+ SWORD HandleSearch;
+ NXT_STATUS Status = NO_ERR;
+ DV_INDEX listIdx, *list;
+ UBYTE *strTemp, *pattern;
+ UBYTE name[FILENAME_LENGTH + 1];
+
+ //Resolve array arguments
+ pattern = cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
+ listIdx = *(DV_INDEX *)(ArgV[2]);
+
+ HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern
+
+ //Count how many files we're going to have
+ while (LOADER_ERR(HandleSearch) == SUCCESS)
+ {
+ matchCount++;
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
+ HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
+ }
+
+ HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern
+
+ oldCount = DV_ARRAY[listIdx].Count; // Check to see how many dope vectors are already in the array (if they passed us a non-blank array of strings)
+
+ Status = cCmdDVArrayAlloc(listIdx, matchCount); // Size the top-level array
+ if(IS_ERR(Status))
+ return Status;
+
+ list = (DV_INDEX*)(VarsCmd.pDataspace + DV_ARRAY[listIdx].Offset); // Get a pointer into the dataspace for the array of DV_INDEXes
+
+ while (LOADER_ERR(HandleSearch) == SUCCESS && !IS_ERR(Status))
+ {
+ pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL); // Close the handle that we automatically opened above
+ // Allocate a new dope vector if one doesn't already exist
+ if(i >= oldCount)
+ Status = cCmdAllocDopeVector(&(list[i]), sizeof(char));
+
+ // Allocate the string buffer for output array[i]
+ if(!IS_ERR(Status))
+ Status = cCmdDVArrayAlloc(list[i], strlen((PSZ)name) + 1);
+
+ if(!IS_ERR(Status))
+ {
+ strTemp = VarsCmd.pDataspace + DV_ARRAY[list[i]].Offset; // Get a pointer into the dataspace for this string
+ strcpy((PSZ)strTemp, (PSZ)name);
+ }
+ i++;
+
+ HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
+ }
+
+ *(SBYTE *)(ArgV[0]) = Status;
+
+ return Status;
+}
+
+#ifdef SIM_NXT
+// Accessors for simulator library code
+SWORD cCmdGetCodeWord(CLUMP_ID Clump, CODE_INDEX Index)
+{
+ if (Clump == NOT_A_CLUMP)
+ {
+ NXT_ASSERT(Index < VarsCmd.CodespaceCount);
+ return (VarsCmd.pCodespace[Index]);
+ }
+ else
+ {
+ NXT_ASSERT(cCmdIsClumpIDSane(Clump));
+#error // CodeStart is now absolute, but not sure how to fix
+ return (((SWORD)VarsCmd.pCodespace[VarsCmd.pAllClumps[Clump].CodeStart + Index]));
+ }
+}
+
+
+UBYTE * cCmdGetDataspace(UWORD *DataspaceSize)
+{
+ if (DataspaceSize)
+ *DataspaceSize = VarsCmd.DataspaceSize;
+ return (VarsCmd.pDataspace);
+}
+
+
+DOPE_VECTOR * cCmdGetDopeVectorPtr()
+{
+ return VarsCmd.MemMgr.pDopeVectorArray;
+}
+
+
+MEM_MGR cCmdGetMemMgr(void)
+{
+ return VarsCmd.MemMgr;
+}
+
+
+ULONG cCmdGetPoolSize()
+{
+ return VarsCmd.PoolSize;
+}
+#endif
+
+#else //!ENABLE_VM
+//
+//Implementations of standard interface if VM is disabled.
+//Place low-level test code here if VM is causing issues.
+//Test code must implement cCmdInit(), cCmdCtrl(), and cCmdExit() at a minimum.
+//Recommend using a pattern like #include "c_cmd_alternate.c"
+//
+
+//!!! !ENABLE_VM implementations really should provide a placeholder function for this pointer
+//IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands;
+#include "c_cmd_alternate.c"
+
+#endif //ENABLE_VM
diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h
new file mode 100644
index 0000000..86949cb
--- /dev/null
+++ b/AT91SAM7S256/Source/c_cmd.h
@@ -0,0 +1,882 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date: 10-07-08 13:22 $
+//
+// Filename $Workfile:: c_cmd.h $
+//
+// Version $Revision: 8 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $
+//
+// Platform C
+//
+
+//
+// File Description:
+// This file contains definitions and prototypes for the VM which runs bytecode
+// programs compatible with LEGO MINDSTORMS NXT Software 1.0.
+//
+
+#ifndef C_CMD
+#define C_CMD
+
+//!!! MAX_HANDLES also defined in m_sched.h
+#ifndef MAX_HANDLES
+#define MAX_HANDLES 16
+#endif
+
+#include "c_cmd_bytecodes.h"
+#define SYSCALL_COUNT 48
+
+extern const HEADER cCmd;
+
+//
+//Standard interface to other modules
+//
+void cCmdInit(void* pHeader);
+void cCmdCtrl(void);
+void cCmdExit(void);
+
+//
+//ARM_NXT vs SIM_NXT
+//These definitions are set up to allow compiling this code for use in a simulated (non-ARM7) environment.
+//If your toolchain doesn't automatically use the __ICCARM__ or __arm__ token, define it to ensure normal compilation.
+//
+#if defined (__ICCARM__) || (defined (__GNUC__) && defined (__arm__))
+#define ARM_NXT
+#else
+#define SIM_NXT
+#endif
+
+//
+//ENABLE_VM toggles compilation the main body of VM code.
+//Define it as 0 to compile alternate implementation for testing (see bottom of c_cmd.c)
+//
+#define ENABLE_VM 1
+#undef ARM_DEBUG
+//
+//VM_BENCHMARK enables extra instrumentation code to measure VM performance.
+//When enabled, a file named "benchmark.txt" is produced every time a program completes.
+//
+#define VM_BENCHMARK (ENABLE_VM && 0) //<-- Toggle to turn on benchmark calculations
+
+#if VM_BENCHMARK
+//Prototype for benchmark recording function
+void cCmdWriteBenchmarkFile();
+#endif
+
+//
+//Run-time assert macros
+//Use these to test for unexpected conditions
+//If expr evaluates as false while running under a debugger,
+// a software breakpoint exception is thrown.
+//NXT_BREAK is just a shortcut for unconditional break.
+//
+//Assert definitions behind WIN_DEBUG only make sense when compiling SIM_NXT
+// under an x86 Windows debugger.
+//
+#if defined WIN_DEBUG
+//"int 3" is a break exception on x86
+#define NXT_ASSERT(expr) if (expr) {} else { __asm {int 3} }
+#define NXT_BREAK NXT_ASSERT(0)
+//
+//Assert definitions behind ARM_DEBUG aren't quite as handy as WIN_DEBUG,
+// but they do record the code line causing the last assert failure.
+//
+#elif defined(ARM_DEBUG)
+#define NXT_ASSERT(expr) if (expr) {}\
+ else\
+ {\
+ VarsCmd.AssertFlag = TRUE;\
+ VarsCmd.AssertLine = __LINE__;\
+ }
+#define NXT_BREAK NXT_ASSERT(0);
+#else
+//Not debugging, so #defined as nothing
+//!!! Note that these definitions means all usages of NXT_ASSERT and NXT_BREAK
+// get stripped out of an unmodified ARM7 build.
+//Unless ARM_DEBUG is enabled, treat them as documentation of expected values.
+#define NXT_ASSERT(expr)
+#define NXT_BREAK
+#endif
+
+//
+//Status byte used to return requests for further action or errors
+//Valid codes #defined in c_cmd.iom
+//!!!JLOFTUS Replace with NXT_STATUS? Same for ASSERTS? Others? Risk factors?
+//
+typedef SBYTE NXT_STATUS;
+
+#if ENABLE_VM
+
+//Intial values for clump records are packed into 4 bytes in the file format.
+#define VM_FILE_CLUMP_REC_SIZE 4
+
+//
+// Definitions for dataspace management, IO Map (IOM) access, and bytecode instruction structure
+//
+
+//Type codes for use in the dataspace table-of-contents (DSTOC)
+typedef UBYTE TYPE_CODE;
+
+enum
+{
+ //VOID type for unused DS elements; never valid to address them from bytecode
+ TC_VOID,
+
+ //Simple scalar integers, equivalent to matching basic types from stdconst.h
+ TC_UBYTE,
+ TC_SBYTE,
+ TC_UWORD,
+ TC_SWORD,
+ TC_ULONG,
+ TC_SLONG, TC_LAST_INT_SCALAR= TC_SLONG,
+
+ //Aggregate types containing one or more scalar
+ TC_ARRAY,
+ TC_CLUSTER,
+
+ //Mutex tracks current holder and any waiting clumps
+ TC_MUTEX,
+ TC_FLOAT, TC_LAST_VALID= TC_FLOAT
+};
+
+//Sizes (in bytes) of each scalar type
+#define SIZE_UBYTE 1
+#define SIZE_SBYTE 1
+#define SIZE_UWORD 2
+#define SIZE_SWORD 2
+#define SIZE_ULONG 4
+#define SIZE_SLONG 4
+#define SIZE_FLOAT 4
+
+//MUTEX record is a struct containing 3 8-bit CLUMP_IDs, packed into 32-bit word
+//See MUTEX_Q typedef
+#define SIZE_MUTEX 4
+
+//Module IDs for IO map addressing
+enum
+{
+ MOD_INPUT,
+ MOD_OUTPUT
+};
+
+//Field IDs for input IOM
+enum
+{
+ IO_IN_TYPE,
+ IO_IN_MODE,
+ IO_IN_ADRAW,
+ IO_IN_NORMRAW,
+ IO_IN_SCALEDVAL,
+ IO_IN_INVALID_DATA
+};
+
+//FPP = Fields Per Port
+#define IO_IN_FPP 6
+#define IO_IN_FIELD_COUNT (IO_IN_FPP * NO_OF_INPUTS)
+
+//Field IDs for input IOM
+enum
+{
+ IO_OUT_FLAGS,
+ IO_OUT_MODE,
+ IO_OUT_SPEED, //AKA "Power"
+ IO_OUT_ACTUAL_SPEED,
+ IO_OUT_TACH_COUNT,
+ IO_OUT_TACH_LIMIT,
+ IO_OUT_RUN_STATE,
+ IO_OUT_TURN_RATIO,
+ IO_OUT_REG_MODE,
+ IO_OUT_OVERLOAD,
+ IO_OUT_REG_P_VAL,
+ IO_OUT_REG_I_VAL,
+ IO_OUT_REG_D_VAL,
+ IO_OUT_BLOCK_TACH_COUNT,
+ IO_OUT_ROTATION_COUNT
+};
+
+#define IO_OUT_FPP 15
+#define IO_OUT_FIELD_COUNT (IO_OUT_FPP * NO_OF_OUTPUTS)
+
+//
+//DS_TOC_ENTRY is a record in the dataspace table of contents
+//The TypeCode describes the data which is stored at Dataspace[DSOffset]
+//
+typedef struct
+{
+ TYPE_CODE TypeCode;
+ UBYTE Flags;
+ SWORD DSOffset;
+} DS_TOC_ENTRY;
+
+//DS_TOC_ENTRY Flags
+//!!! Yes, there's only one flag defined for an 8-bit field.
+//ARM7 alignment rules means those bits would otherwise just be padding, anyway.
+#define DS_DEFAULT_DEFAULT 1 //This entry has no default value in file; fill with zero at activation time
+
+//DS_ELEMENT_ID (AKA "DS item ID") indexes DataspaceTOC
+typedef UWORD DS_ELEMENT_ID;
+
+//Special flag value used for opcode-specific default behavior when real dataspace argument is not provided
+#define NOT_A_DS_ID 0xFFFF
+
+//Macro to bump DS_ELEMENT_IDs +1 with a cast (mostly to quash annoying warnings)
+#define INC_ID(X) ((DS_ELEMENT_ID)(X + 1))
+
+//DATA_ARG may contain a DS_ELEMENT_ID or encoded IO map address
+typedef UWORD DATA_ARG;
+
+//CODE_WORD is a single indexable element of the codespace
+typedef SWORD CODE_WORD;
+
+//CODE_INDEX indexes codespaces for opcodes and args