Merge git://github.com/r0ket/r0ket

firmware/applications/Makefile

@@ -2,7 +2,16 @@
 # User configuration and firmware specific object files	
 ##########################################################################
 
-OBJS = default.o
+OBJS =
+
+ifeq "$(APP)" "initial"
+NODEFAULT = y
+endif
+
+ifndef NODEFAULT
+OBJS += default.o
+endif
+
 OBJS += $(foreach mod,$(APP),$(mod).o)
 
 SRCS = $(foreach mod,$(APP),$(mod).c)
@@ -53,13 +62,15 @@ LOBJ=loadable_$(LAPP).o
 
 $(LOBJ):
 	$(CC) $(CFLAGS) -o $@ $(LSRC)
-	$(RM) $(LIBFILE)
 
 OBJS += $(LOBJ)
 endif
 
 ifeq "$(APP)" "tester"
-TSRCS = $(wildcard $(APP)/*.c)
+ifndef TYPE
+TYPE=$(APP)
+endif
+TSRCS = $(wildcard $(TYPE)/*.c)
 TOBJS = $(foreach mod,$(TSRCS),$(subst .c,.o,$(mod)))
 
 TWRAP=tester.gen
@@ -75,13 +86,14 @@ OBJS += $(TOBJS)
 endif
 
 $(LIBFILE): $(OBJS) $(WRAPOBJ)
+	$(RM) -f $@
 	$(AR) rcs $@ $(OBJS) $(WRAPOBJ)
 	
 %.o : %.c
 	$(CC) $(CFLAGS) -o $@ $<
 
 clean:
-	rm -f $(OBJS) $(WRAPOBJ) $(WRAPSRC) $(LIBFILE) *.o tester/*.o
+	rm -f $(OBJS) $(WRAPOBJ) $(WRAPSRC) $(LIBFILE) *.o */*.o
 
 %.c:
 	@echo 

firmware/applications/default.c

@@ -9,105 +9,15 @@
 
 /**************************************************************************/
 
-FATFS FatFs[_VOLUMES];          /* File system object for logical drive */
-
-#define CONFIGLEN 2
-int lcdInitConfig(){
-    FIL file;            /* File objects */
-    BYTE buf[CONFIGLEN];
-    UINT readbytes;
-    int res;
-
-    lcdFill(0); // clear display buffer
-    res=f_mount(0, &FatFs[0]);
-    lcdPrint("mount:");
-    lcdPrintln(f_get_rc_string(res));
-    if(res){
-        return 1;
-    };
-
-    res=f_open(&file, "r0ket.cfg", FA_OPEN_EXISTING|FA_READ);
-    lcdPrint("open:");
-    lcdPrintln(f_get_rc_string(res));
-    if(res){
-        if(res==FR_NO_FILESYSTEM)
-            return 1;
-
-        lcdPrintln("new r0ket.cfg...");
-
-        res=f_open(&file, "r0ket.cfg", FA_OPEN_ALWAYS|FA_WRITE);
-        lcdPrint("create:");
-        lcdPrintln(f_get_rc_string(res));
-        if(res){
-            return 1;
-        };
-
-        buf[0]='0';
-        buf[1]='0';
-        res = f_write(&file, buf, 2, &readbytes);
-        lcdPrint("write:");
-        lcdPrintln(f_get_rc_string(res));
-        if(res){
-            return 1;
-        };
-
-        lcdPrint("wrote:");
-        lcdPrintInt(readbytes);
-        lcdPrintln("b");
-
-        res=f_close(&file);
-        lcdPrint("close:");
-        lcdPrintln(f_get_rc_string(res));
-        if(res){
-            return 1;
-        };
-        return 2; // created. Still show screen
-    };
-
-    for(int i=0;i<CONFIGLEN;i++)
-        buf[i]=0;
-
-    res = f_read(&file, buf, 2, &readbytes);
-    lcdPrint("read:");
-    lcdPrintln(f_get_rc_string(res));
-    if(res){
-        return 1;
-    };
-
-    lcdPrint("r: ");
-    lcdPrintCharHex(buf[0]);
-    lcdPrintCharHex(buf[1]);
-    lcdNl();
-
-    if(buf[0] == '1')
-        lcdToggleFlag(LCD_INVERTED);
-    if(buf[1] == '1')
-        lcdToggleFlag(LCD_MIRRORX);
-
-    res=f_close(&file);
-    lcdPrint("close:");
-    lcdPrintln(f_get_rc_string(res));
-    if(res){
-        return 1;
-    };
-    return 0;
-};
-
-
 void main_default(void) {
-    systickInit(10);
+    systickInit(SYSTICKSPEED);
 
     if(getInputRaw()==BTN_ENTER){
         ISPandReset();
     };
 
-    font=&Font_7x8;
+    readConfig();
-    if(lcdInitConfig()){
+    applyConfig();
-        lcdDisplay();
-        getInputWait();
-    }else{
-        lcdDisplay();
-    };
     randomInit();
 
     return;
@@ -118,7 +28,7 @@ void tick_default(void) {
     static int ctr;
     ctr++;
     incTimer();
-    if(ctr>100){
+    if(ctr>1000/SYSTICKSPEED){
         if(!adcMutex){
             VoltageCheck();
             LightCheck();
@@ -129,11 +39,11 @@ void tick_default(void) {
     };
 
     if(isNight())
-        backlightSetBrightness(100);
+        backlightSetBrightness(globalconfig.backlightvalue);
     else
         backlightSetBrightness(0);
 
-    if(ctr%5==0){
+    if(ctr%(50/SYSTICKSPEED)==0){
 
         if(GetVoltage()<3600
 #ifdef SAFE
@@ -142,7 +52,7 @@ void tick_default(void) {
                 ){
             IOCON_PIO1_11 = 0x0;
             gpioSetDir(RB_LED3, gpioDirection_Output);
-            if( (ctr/5)%10 == 1 )
+            if( (ctr/(50/SYSTICKSPEED))%10 == 1 )
                 gpioSetValue (RB_LED3, 1);
             else
                 gpioSetValue (RB_LED3, 0);

firmware/applications/iggy.c

@@ -142,7 +142,7 @@ void main_iggy(void) {
 	    gpioSetValue (RB_LCD_BL, 0); 
 	    SCB_SCR |= SCB_SCR_SLEEPDEEP;
 	    PMU_PMUCTRL = PMU_PMUCTRL_DPDEN_DEEPPOWERDOWN;
-#ifdef ARM
+#ifdef __arm__
 	    __asm volatile ("WFI");
 #endif
 	}else{

firmware/applications/initial.c

@@ -11,6 +11,9 @@
 
 void main_initial(void) {
     char key=BTN_NONE;
+
+    systickInit(SYSTICKSPEED);
+
     gpioSetValue (RB_LED0, 0); 
     gpioSetValue (RB_LED1, 0); 
     gpioSetValue (RB_LED2, 0); 
@@ -89,6 +92,7 @@ void main_initial(void) {
 void tick_initial(void){
     static int foo=0;
     static int toggle=0;
+    incTimer();
     if(foo++>80){
         toggle=1-toggle;
         foo=0;

firmware/applications/serial/config.c

@@ -0,0 +1 @@
+../tester/config.c

firmware/applications/serial/serial.c

@@ -0,0 +1,116 @@
+#include <sysinit.h>
+
+#include "basic/basic.h"
+
+#include "lcd/lcd.h"
+#include "lcd/print.h"
+
+#include "funk/nrf24l01p.h"
+
+#include "core/usbcdc/usb.h"
+#include "core/usbcdc/usbcore.h"
+#include "core/usbcdc/usbhw.h"
+#include "core/usbcdc/cdcuser.h"
+#include "core/usbcdc/cdc_buf.h"
+
+#include <string.h>
+
+#if CFG_USBMSC
+#error "MSC is defined
+#endif
+
+#if !CFG_USBCDC
+#error "CDC is not defined
+#endif
+
+/**************************************************************************/
+
+volatile unsigned int lastTick;
+int puts(const char * str)
+{
+  // There must be at least 1ms between USB frames (of up to 64 bytes)
+  // This buffers all data and writes it out from the buffer one frame
+  // and one millisecond at a time
+    if (USB_Configuration) 
+    {
+      while(*str)
+        cdcBufferWrite(*str++);
+      // Check if we can flush the buffer now or if we need to wait
+      unsigned int currentTick = systickGetTicks();
+      if (currentTick != lastTick)
+      {
+        uint8_t frame[64];
+        uint32_t bytesRead = 0;
+        while (cdcBufferDataPending())
+        {
+          // Read up to 64 bytes as long as possible
+          bytesRead = cdcBufferReadLen(frame, 64);
+          USB_WriteEP (CDC_DEP_IN, frame, bytesRead);
+          systickDelay(1);
+        }
+        lastTick = currentTick;
+      }
+    }
+  return 0;
+}
+
+
+void f_ser(void) {
+    //lastTick = systickGetTicks();   // Used to control output/printf timing
+    CDC_Init();                     // Initialise VCOM
+    USB_Init();                     // USB Initialization
+    lcdPrintln("preconnect");
+    USB_Connect(TRUE);              // USB Connect
+    lcdPrintln("postconnect");
+    // Wait until USB is configured or timeout occurs
+    uint32_t usbTimeout = 0; 
+//    while ( usbTimeout < CFG_USBCDC_INITTIMEOUT / 10 ) {
+//      if (USB_Configuration) break;
+//      delayms(10);             // Wait 10ms
+//      usbTimeout++;
+//    }
+    lcdPrintln("fini");
+};
+
+void f_disconnect(void) {
+    USB_Connect(FALSE);
+};
+
+#define LEN 10
+void f_sread(){
+    uint8_t buf[LEN+1];
+    int l=LEN;
+
+    lcdPrint("Bytes:");
+    CDC_OutBufAvailChar (&l);
+    lcdPrintInt(l);
+    lcdNl();
+
+    lcdPrint("read:");
+    CDC_RdOutBuf (buf, &l);
+    lcdPrintInt(l);
+    lcdNl();
+
+    buf[l]=0;
+    lcdPrintln(buf);
+};
+
+void f_echo(){
+    uint8_t buf[2] = {0,0};
+    int l;
+    while(1){
+        CDC_OutBufAvailChar(&l);
+        if( l ){
+            l = 1;
+            CDC_RdOutBuf (buf, &l);
+            puts(buf);
+        }
+        //puts("hello world\r\n");
+        //delayms(1);
+    }
+};
+
+void f_say(){
+        puts("hello world\r\n");
+};
+

firmware/applications/serial/util.c

@@ -0,0 +1 @@
+../tester/util.c

firmware/applications/serial/uuid.c

@@ -0,0 +1 @@
+../tester/uuid.c

firmware/applications/tester/config.c

@@ -0,0 +1,38 @@
+#include <sysinit.h>
+
+#include "basic/basic.h"
+
+#include "lcd/print.h"
+#include "lcd/display.h"
+
+#include "filesystem/ff.h"
+
+#include <string.h>
+
+/**************************************************************************/
+
+void readcfg(void) {
+    readConfig();
+};
+
+void savecfg(void){
+    saveConfig();
+};
+
+void applycfg(void){
+    applyConfig();
+};
+
+void show(void){
+    lcdClear();
+    lcdPrint("time:"); lcdPrintInt(globalconfig.time); lcdNl();
+    lcdPrint("btrig:"); lcdPrintInt(globalconfig.backlighttrigger); lcdNl();
+    lcdPrint("bval:"); lcdPrintInt(globalconfig.backlightvalue); lcdNl();
+    lcdPrint("lcd:"); lcdPrintInt(globalconfig.lcdstate); lcdNl();
+    lcdPrint("priv:"); lcdPrintInt(globalconfig.privacy); lcdNl();
+    lcdRefresh();
+};
+
+void lcdmirror(void){
+    lcdToggleFlag(LCD_MIRRORX);
+};

firmware/applications/tester/util.c

@@ -33,10 +33,11 @@ void adc_light(void) {
     int dx=0;
     int dy=8;
     dx=DoString(0,dy,"Light:");
-    DoString(0,dy+8,"Night:");
+    DoString(0,dy+16,"Night:");
     while ((getInputRaw())==BTN_NONE){
         DoInt(dx,dy,GetLight());
-        DoInt(dx,dy+8,isNight());
+        DoInt(dx,dy+16,isNight());
+        DoInt(dx,dy+8,globalconfig.backlighttrigger);
         lcdDisplay();
     };
     dy+=8;
@@ -74,7 +75,8 @@ void msc_menu(void){
     DoString(0,8,"MSC Enabled.");
     lcdDisplay();
     usbMSCInit();
-    while(!getInputRaw())delayms(10);
+    getInputWaitRelease();
+    getInputWait();
     DoString(0,16,"MSC Disabled.");
     usbMSCOff();
 };

firmware/basic/Makefile

@@ -18,6 +18,7 @@ OBJS += ecc.o
 OBJS += byteorder.o
 OBJS += random.o
 OBJS += idle.o
+OBJS += config.o
 
 LIBNAME=basic
 

firmware/basic/basic.h

@@ -182,5 +182,24 @@ void handleMenu(const struct MENU *the_menu);
 
 #include "basic/idle.h"
 
-#endif
+// config.c
 
+struct config_t {
+	time_t     time;
+	uint16_t   backlighttrigger;
+	char       backlightvalue; 
+	char       lcdstate;
+	char       privacy;
+} __attribute__((__packed__));
+
+typedef struct config_t CONFIG;
+
+extern CONFIG globalconfig;
+
+int readConfig(void);
+int saveConfig(void);
+int applyConfig(void);
+
+#define SYSTICKSPEED 10
+
+#endif

firmware/basic/config.c

@@ -0,0 +1,81 @@
+#include <sysinit.h>
+#include "basic/basic.h"
+
+#include "lcd/lcd.h"
+#include "lcd/fonts/smallfonts.h"
+#include "lcd/print.h"
+#include "filesystem/ff.h"
+#include "basic/random.h"
+
+CONFIG globalconfig = { 0,310,50,0,0};
+
+#define CONFFILE "r0ket.cfg"
+
+/**************************************************************************/
+
+int saveConfig(void){
+    FIL file;            /* File object */
+    UINT writebytes;
+    int res;
+
+	res=f_open(&file, CONFFILE, FA_OPEN_ALWAYS|FA_WRITE);
+	lcdPrint("create:");
+	lcdPrintln(f_get_rc_string(res));
+	if(res){
+		return 1;
+	};
+
+	res = f_write(&file, &globalconfig, sizeof(CONFIG), &writebytes);
+	lcdPrint("write:");
+	lcdPrintln(f_get_rc_string(res));
+	if(res){
+		return 1;
+	};
+
+	lcdPrint("wrote:");
+	lcdPrintInt(writebytes);
+	lcdPrintln("b");
+
+	res=f_close(&file);
+	lcdPrint("close:");
+	lcdPrintln(f_get_rc_string(res));
+	if(res){
+		return 1;
+	};
+	return 0;
+};
+
+int readConfig(void){
+    FIL file;            /* File object */
+    UINT readbytes;
+    int res;
+
+    lcdFill(0); // clear display buffer
+
+    res=f_open(&file, CONFFILE, FA_OPEN_EXISTING|FA_READ);
+    lcdPrint("open:");
+    lcdPrintln(f_get_rc_string(res));
+    if(res){
+        return 1;
+    };
+
+    res = f_read(&file, &globalconfig, sizeof(CONFIG), &readbytes);
+    lcdPrint("read:");
+    lcdPrintln(f_get_rc_string(res));
+    if(res){
+        return 1;
+    };
+
+
+    res=f_close(&file);
+    lcdPrint("close:");
+    lcdPrintln(f_get_rc_string(res));
+    if(res){
+        return 1;
+    };
+    return 0;
+};
+
+int applyConfig(){
+	return 0;
+};

firmware/basic/idle.c

@@ -4,9 +4,10 @@
 #include "lcd/print.h"
 
 QUEUE the_queue;
-#ifdef ARM
+#ifdef __arm__
 volatile uint32_t _timectr=0;
 #else
+#error "foo"
 extern uint32_t simTimeCounter();
 #define _timectr (simTimeCounter())
 #endif
@@ -18,8 +19,10 @@ void work_queue(void){
 	int start;
 
 	if (the_queue.qstart == the_queue.qend){
-#ifdef ARM
+#ifdef __arm__
 		__asm volatile ("WFI");
+#else
+            delayms(SYSTICKSPEED);
 #endif
 		return;
 	};
@@ -33,11 +36,13 @@ void work_queue(void){
 };
 
 void delayms_queue(uint32_t ms){
-	int end=_timectr+ms/10;
+	int end=_timectr+ms/SYSTICKSPEED;
 	do {
 		if (the_queue.qstart == the_queue.qend){
-#ifdef ARM
+#ifdef __arm__
 			__asm volatile ("WFI");
+#else
+            delayms(SYSTICKSPEED);
 #endif
 		}else{
 			work_queue();
@@ -46,10 +51,13 @@ void delayms_queue(uint32_t ms){
 };
 
 void delayms_power(uint32_t ms){
+    ms/=SYSTICKSPEED;
     ms+=_timectr;
 	do {
-#ifdef ARM
+#ifdef __arm__
 			__asm volatile ("WFI");
+#else
+            delayms(SYSTICKSPEED);
 #endif
 	} while (ms >_timectr);
 };

firmware/basic/menu.c

@@ -76,6 +76,7 @@ void handleMenu(const struct MENU *the_menu) {
                 lcdClear();
                 lcdPrintln("Called...");
                 lcdRefresh();
+                getInputWaitRelease();
                 if (the_menu->entries[menuselection]->callback!=NULL)
                     the_menu->entries[menuselection]->callback();
                 lcdDisplay();
@@ -86,6 +87,7 @@ void handleMenu(const struct MENU *the_menu) {
                 /* no button pressed */
                 break;
         }
+        getInputWaitRelease();
 
     }
     return;

firmware/basic/night.c

@@ -7,6 +7,8 @@
 uint32_t light=300*HYST;
 char _isnight=1;
 
+#define threshold globalconfig.backlighttrigger
+
 void LightCheck(void){
     int iocon;
     char iodir;
@@ -25,16 +27,14 @@ void LightCheck(void){
     gpioSetDir(RB_LED3, iodir);
     IOCON_PIO1_11=iocon;
 
-    static uint16_t threshold=0;
+    if(threshold==0){ // uninitialized?
-    if(threshold==0){
+        threshold=320;
-        // read threshold from config
-        threshold=320 * HYST;
     };
 
-    if(_isnight && light>(threshold+RANGE*HYST))
+    if(_isnight && light/HYST>(threshold+RANGE))
         _isnight=0;
 
-    if(!_isnight && light<threshold)
+    if(!_isnight && light/HYST<threshold)
         _isnight=1;
 };
 

firmware/basic/reinvoke_isp.c

@@ -76,10 +76,16 @@ void EnableWatchdog(uint32_t ms){
 
 void ISPandReset(void){
 #if CFG_USBMSC
-    if(usbMSCenabled){
+    if(usbMSCenabled&USB_MSC_ENABLEFLAG){
         usbMSCOff();
         delayms(500);
     };
+#endif
+#if CFG_USBCDC
+    if(usbMSCenabled&USB_CDC_ENABLEFLAG){
+        USB_Connect(FALSE);
+        delayms(500);
+    };
 #endif
     EnableWatchdog(1000*5);
     ReinvokeISP();

firmware/core/lpc134x.h

@@ -1937,7 +1937,7 @@ typedef struct
 
 #define NVIC                                      ((NVIC_Type *) NVIC_BASE_ADDRESS)
 
-#ifdef ARM
+#ifdef __arm__
 static inline void __enable_irq()                 { __asm volatile ("cpsie i"); }
 static inline void __disable_irq()                { __asm volatile ("cpsid i"); }
 #else

firmware/filesystem/ff.h

@@ -330,6 +330,7 @@ int ff_del_syncobj (_SYNC_t);		/* Delete a sync object */
 
 /* Utility functions */
 const char* f_get_rc_string (FRESULT rc);
+void fsInit();
 
 #ifdef __cplusplus
 }

firmware/filesystem/util.c

@@ -1,5 +1,7 @@
 #include <ff.h>
 
+FATFS FatFs;          /* File system object for logical drive */
+
 const TCHAR *rcstrings =
     _T("OK\0DISK_ERR\0INT_ERR\0NOT_READY\0NO_FILE\0NO_PATH\0INVALID_NAME\0")
     _T("DENIED\0EXIST\0INVALID_OBJECT\0WRITE_PROTECTED\0INVALID_DRIVE\0")
@@ -16,3 +18,7 @@ const char* f_get_rc_string (FRESULT rc) {
     return p;
 }
 
+
+void fsInit(){
+    f_mount(0, &FatFs);
+};

firmware/lcd/display.c

@@ -13,7 +13,7 @@
 /**************************************************************************/
 
 uint8_t lcdBuffer[RESX*RESY_B];
-int lcd_layout = 0;
+#define lcd_layout globalconfig.lcdstate
 uint32_t intstatus; // Caches USB interrupt state
                     // (need to disable MSC while displaying)
 
@@ -79,9 +79,23 @@ void lcdInit(void) {
 
     lcd_select();
 
+/* Small Nokia 1200 LCD docs:
+ *           clear/ set
+ *  on       0xae / 0xaf
+ *  invert   0xa6 / 0xa7
+ *  mirror-x 0xA0 / 0xA1
+ *  mirror-y 0xc7 / 0xc8
+ *
+ *  0x20+x contrast (0=black - 0x2e)
+ *  0x40+x offset in rows from top (-0x7f)
+ *  0x80+x contrast? (0=black -0x9f?)
+ *  0xd0+x black lines from top? (-0xdf?)
+ *
+ */
+
     lcdWrite(TYPE_CMD,0xE2);
     delayms(5);
-    lcdWrite(TYPE_CMD,0xAF);
+    lcdWrite(TYPE_CMD,0xAF); // Display ON
     lcdWrite(TYPE_CMD,0xA4);
     lcdWrite(TYPE_CMD,0x2F);
     lcdWrite(TYPE_CMD,0xB0);
@@ -252,3 +266,4 @@ void lcdShift(int x, int y, bool wrap) {
     while(y-->0)
 			lcdShiftV(dir, wrap);
 }
+

firmware/main.c

@@ -23,6 +23,8 @@ int main(void) {
     // initialise basic badge functions
     rbInit();
 
+    fsInit();
+  
     lcdInit(); // display
 
     lcdFill(0);

firmware/usb/usbmsc.c

@@ -85,7 +85,7 @@ void usbMSCInit(void) {
 
   (*rom)->pUSBD->init(&DeviceInfo); /* USB Initialization */
   (*rom)->pUSBD->connect(true);     /* USB Connect */
-  usbMSCenabled=1;
+  usbMSCenabled|=USB_MSC_ENABLEFLAG;
 }
 
 #if CFG_USBMSC
@@ -96,6 +96,6 @@ void USB_IRQHandler() {
 
 void usbMSCOff(void) {
   (*rom)->pUSBD->connect(false);     /* USB Disconnect */
-  usbMSCenabled=0;
+  usbMSCenabled&=~USB_MSC_ENABLEFLAG;
 }
 

firmware/usb/usbmsc.h

@@ -39,6 +39,8 @@
 
 #include "projectconfig.h"
 
+#define USB_MSC_ENABLEFLAG (1<<0)
+#define USB_CDC_ENABLEFLAG (1<<1)
 extern char usbMSCenabled;
 void usbMSCWrite(uint32_t offset, uint8_t src[], uint32_t length);
 void usbMSCRead(uint32_t offset, uint8_t dst[], uint32_t length);

firmware/usbcdc/usbhw.c

@@ -27,6 +27,7 @@
 #include "usbcore.h"
 #include "usbuser.h"
 
+#include "usb/usbmsc.h"
 
 /*    
  *    USB and IO Clock configuration only. 
@@ -63,8 +64,8 @@ void USBIOClkConfig( void )
   SCB_USBCLKSEL = SCB_USBCLKSEL_SOURCE_USBPLLOUT;
 
   // Set USB pin functions
-  IOCON_PIO0_1 &= ~IOCON_PIO0_1_FUNC_MASK;
+//  IOCON_PIO0_1 &= ~IOCON_PIO0_1_FUNC_MASK;
-  IOCON_PIO0_1 |= IOCON_PIO0_1_FUNC_CLKOUT;           // CLK OUT
+//  IOCON_PIO0_1 |= IOCON_PIO0_1_FUNC_CLKOUT;           // CLK OUT
   IOCON_PIO0_3 &= ~IOCON_PIO0_3_FUNC_MASK;
   IOCON_PIO0_3 |= IOCON_PIO0_3_FUNC_USB_VBUS;         // VBus
   IOCON_PIO0_6 &= ~IOCON_PIO0_6_FUNC_MASK;
@@ -205,6 +206,10 @@ void USB_Init (void)
 
 void USB_Connect (uint32_t con) 
 {
+    if(con)
+        usbMSCenabled|=USB_CDC_ENABLEFLAG;
+    else
+        usbMSCenabled&=~USB_CDC_ENABLEFLAG;
   WrCmdDat(CMD_SET_DEV_STAT, DAT_WR_BYTE(con ? DEV_CON : 0));
 }