--- p2kproc.cpp-orig	2005-03-02 15:49:30.000000000 -0300
+++ p2kproc.cpp	2005-07-08 18:45:22.541263000 -0300
@@ -694,9 +694,36 @@
 
 void P2kProc::drv_switchP2K(char * st)
 {
-	char cmd[1024];
-	sprintf(cmd,"echo \"AT+MODE=8\" > %s", (st == 0) ? ACMdev : st);
-	system(cmd);
+        FUNC("p2k_connect");
+        phoneHandle=0;
+        phoneHandle = usb_open(phone);
+        if (phoneHandle==0) printf("Unable to open phone\n");
+        if (usb_set_configuration(phoneHandle,1)) printf("Unable to set configuration");
+        if (usb_claim_interface(phoneHandle, 1)) printf("Unable to claim the interface");
+        // Switching to p2k
+        char buf[10240];
+        int res;
+        do
+                res=usb_bulk_read(phoneHandle,0x82, buf, 100,100);
+        while (res>0);
+
+        strcpy(buf,"AT+MODE=8\r\n");
+        res=usb_bulk_write(phoneHandle,1,buf,strlen(buf),1000);
+        if (res>0)
+        {
+                printf ("-> mOto DRiveR =OK= bytes: %d <-\n", res );
+                res=usb_bulk_read(phoneHandle,0x82, buf, 100,1000);
+                if (res>0)
+                {
+                        buf[res]=0;
+                        printf("Feedback: %s\n",buf);
+                }
+                usb_release_interface(phoneHandle, 1);
+                usb_close (phoneHandle);
+         } else {
+                printf ("-> mOto DRiveR =ERROR= AT \n");
+                printf("%s\n", usb_strerror());
+        }
 }
 
 // Open phone