int main() { LibSerial::SerialStream ss; ss.Open("/dev/ttyACM0"); //serial/by-id/usb-LeafLabs_Maple-if00"); if ( !ss.good() ) { std::cout << "Not good" << std::endl; exit(1); } ss.SetCharSize( LibSerial::SerialStreamBuf::CHAR_SIZE_8 ) ; if ( !ss.good() ) { std::cout << "can't set char size" << std::endl; exit(1); } char c = 0x1; write_letter(ss,l_a); usleep(1000000); write_letter(ss,l_b); //ss.write(&c, 0); }
int write_byte( LibSerial::SerialStream& serial_port, char data) { if( serial_port.good() ) { serial_port.write( &data, 1); return 1; } else std::cout << "Unable to write to serial port" << std::endl; return 0; }
void init_serial() { // Open the serial port. const char *const SERIAL_PORT_DEVICE = "/dev/ttyACM0"; serial_port.Open(SERIAL_PORT_DEVICE); CHECK_S("Could not open serial port") //SET_AND_CHECK( SetBaudRate, BAUD_9600, "Could not set the baud rate." ); SET_AND_CHECK( SetCharSize, CHAR_SIZE_8, "Could not set the character size." ); SET_AND_CHECK( SetParity , PARITY_NONE, "Could not disable the parity." ); // Set the number of stop bits serial_port.SetNumOfStopBits(1); CHECK_S("Could not set the number of stop bits") // Turn on hardware flow control serial_port.SetFlowControl(LibSerial::SerialStreamBuf::FLOW_CONTROL_NONE); CHECK_S("Could not use hardware flow control") }
inline void write_letter(LibSerial::SerialStream& ss, const char* l) { for ( char c = 0; c < 25; ++c ) { char d = (c<<1); if ( l[c] ) d |= 1; ss.write(&d, 1); } }
/** * release FUSE function * @param * @see fuse_operations structure from fuse.h * @return */ static int usb_release(const char *path, struct fuse_file_info *fi) { //printf("[+] usb_release start\n"); (void) fi; if(strcmp(path, devname) != 0) return -ENOENT; s.Close(); //printf("[+] usb_release end\n"); return 0; }
/** * open FUSE function * @param * @see fuse_operations structure from fuse.h * @return */ static int usb_open(const char *path, struct fuse_file_info *fi) { //printf("[+] usb_open start\n"); if(strcmp(path, devname) != 0) { //printf("[x] usb_open path error: %s\n", path); return -ENOENT; } /*if((fi->flags & 3) != O_RDONLY) { //printf("[x] usb_open flags error: %x\n", fi->flags); return -EACCES; }*/ s.Open("/dev/ttyUSB0"); //printf("[+] usb_open end\n"); return 0; }
int main() { init_serial(); char package[5]; package[0] = 'a'; package[4] = 0; int v[3]; Joystick joystick("/dev/input/js0"); if (!joystick.isFound()) { std::cerr << "no joystick found" << std::endl; return 1; } while (true) { int axis0_pos, axis1_pos, axis3_pos; float v_ang, v_tan, v_norm; JoystickEvent event; if (joystick.sample(&event)) { if ((event.isAxis()) && (event.number == 1)) { axis1_pos = event.value; if (axis1_pos > 4000 || axis1_pos < -4000) { v_tan = (-(float)axis1_pos / 1000.0); } else { v_tan = 0; } v[0]=v_tan; std::cout << "v_tan"<< v_tan << std::endl; } if ((event.isAxis()) && (event.number == 0)) { axis0_pos = event.value; if (axis0_pos > 4000 || axis0_pos < -4000) { v_norm = (-(float)axis0_pos / 1000.0); } else { v_norm = 0; } v[1]=v_norm; std::cout << "v_norm" <<v_norm << std::endl; } if ((event.isAxis()) && (event.number == 3)) { axis3_pos = event.value; if ((axis3_pos > 7000) || (axis3_pos < -1000)) { v_ang = (-(float)axis3_pos / 1000.0); } else { v_ang = 0; } v[2]=v_ang; std::cout << "v_ang" << v_ang << std::endl; } for(int i = 0; i< 3; i++){ package[i+1] = v[i]; } for(int i = 0; i < 5; i++){ serial_port.write(&package[i], 1); std::cerr << package[i] << " "; } sleep(0.01); } } return EXIT_SUCCESS; }
int setup_serial_port( LibSerial::SerialStream& serial_port, const char* SERIAL_PORT_DEVICE, LibSerial::SerialStreamBuf::BaudRateEnum baud_rate ) { using namespace LibSerial; //Open the port SERIAL_PORT_DEVICE serial_port.Open( SERIAL_PORT_DEVICE ); if ( ! serial_port.good() ) { std::cout << "Not able to open serial port at " << SERIAL_PORT_DEVICE << std::endl; return 0; } //Set Baudrate serial_port.SetBaudRate( baud_rate ); if ( ! serial_port.good() ) { std::cout << "Unable to set Baud Rate " << std::endl; return 0; } //Set Character Size serial_port.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ); if( ! serial_port.good() ) { std::cout << "Unable to set Char Size 8" << std::endl; return 0; } //Set Parity serial_port.SetParity( SerialStreamBuf::PARITY_NONE ); if( ! serial_port.good() ) { std::cout << "Unable to set Parity None " << std::endl; return 0; } //Set Number of Stop Bits serial_port.SetNumOfStopBits( 1 ); if( ! serial_port.good() ) { std::cout << "Unable to set Num of Stop Bits 1 " << std::endl; return 0; } //Set Hardware Flow Control serial_port.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ); if( ! serial_port.good() ) { std::cout << "Unable to set Hardware Flow Control None" << std::endl; return 0; } return 1; }
/** * Reads data from an opened serial line over USB from HBOOT supporting read_emmc function * @param s an opened serial-over-USB device * @param outBuf a buffer to save read data * @param offset the offset in bytes from the start of /data partition * @param nBytes the nb of bytes to read * @see None * @return the nb of read bytes of -1 if error */ int read_emmc(LibSerial::SerialStream *s, char* outBuf, unsigned int offset, unsigned int nBytes) { //printf("[+] read_emmc start\n"); const int LINE_SIZE = 256; char input[LINE_SIZE]; char output[LINE_SIZE]; memset(input, 0, LINE_SIZE); memset(output, 0, LINE_SIZE); void* res = NULL; char* prompt = NULL; unsigned int loop = 0; char indexcache_filename[256]; char cache_filename[256]; sprintf(indexcache_filename, "%s/%s", cache_dir, INDEXCACHE_FILENAME); sprintf(cache_filename, "%s/%s", cache_dir, CACHE_FILENAME); /* Some check to make it simple. If needed, the caller must ask for more and ignore what it does not want. */ if ((nBytes < 512) || (offset % 512 != 0) || (nBytes % 512 != 0)) { printf("[+] read_emmc: not supporting: offset=%d, nBytes=%d\n", offset, nBytes); return -1; //Not supported for now. } /* Determine what sectors to read from all over (cached and HBOOT) */ unsigned int nBlocks = nBytes / 512; unsigned int startBlock = offset / 512; printf("[+] read_emmc: blocks to get from everything: [%d, %d]\n", startBlock, startBlock+nBlocks-1); // Prepare our cache files if ((indexcache_fd = open(indexcache_filename, O_RDWR, S_IRWXU|S_IRWXG|S_IRWXO)) == -1) { printf("[x] read_emmc: can not open index cache file: %s\n", indexcache_filename); return -1; } if ((cache_fd = open(cache_filename, O_RDWR, S_IRWXU|S_IRWXG|S_IRWXO)) == -1) { printf("[x] read_emmc: can not open cache file: %s\n", cache_filename); return -1; } //printf("Opened handles: index=%d, cache=%d\n", indexcache_fd, cache_fd); /* Determine what sectors to read from all over from cached */ // Do we have some data already? char* cacheTest = (char*)malloc(nBlocks*sizeof(char)); if (!cacheTest) { printf("[x] read_emmc: malloc failed for reading index cache\n"); return -1; } if (lseek(indexcache_fd, startBlock, SEEK_SET) != startBlock) { printf("[x] read_emmc: can not seek in index cache file for reading: %s\n", indexcache_filename); return -1; } if (read(indexcache_fd, cacheTest, nBlocks) != (int)nBlocks) { printf("[x] read_emmc: read failed for index cache\n"); return -1; } unsigned int startBlockCached = startBlock; unsigned int nBlocksCached = 0; for (unsigned int i=0; i<nBlocks; i++) { if (cacheTest[i] == 0x00) break; nBlocksCached++; } if (cacheTest) { free(cacheTest); cacheTest = NULL; } /* Conclude what sectors to read from all over from HBOOT */ unsigned int startBlockHBOOT = startBlock+nBlocksCached; unsigned int nBlocksHBOOT = nBlocks-nBlocksCached; unsigned int nBlocksHBOOTPerRead = nBlocksHBOOT; unsigned int nBytesReadHBOOT = 0; unsigned int nBytesReadPerLineHBOOT = 0; printf("[+] read_emmc: blocks to get from cache: [%d, %d] - %d blocks = %d bytes\n", startBlockCached, startBlockCached+nBlocksCached-1, nBlocksCached, nBlocksCached*512); printf("[+] read_emmc: blocks to get from HBOOT: [%d, %d] - %d blocks = %d bytes\n", startBlockHBOOT, startBlockHBOOT+nBlocksHBOOT-1, nBlocksHBOOT, nBlocksHBOOT*512); /* Get data from cache */ if (nBlocksCached > 0) { printf("[+] read_emmc: getting data from cache\n"); // Prepare temporary buffer char* outBufTmpCached = (char*)malloc(nBlocksCached*512*sizeof(char)); if (!outBufTmpCached) { printf("[x] read_emmc: malloc failed for outBufTmpCached\n"); return -1; } off_t res1 = lseek(cache_fd, startBlockCached*512, SEEK_SET); if (res1 != startBlockCached*512) { //We support userdata partition size < 2^32 ~ 4GB printf("[x] usb_read: can not seek in cache file for outBufTmpCached: %s\n", cache_filename); perror("lseek failed: "); return -1; } //printf("lseek success: %d\n", (int)res1); ssize_t res = read(cache_fd, outBufTmpCached, nBlocksCached*512); if (res != (int)(nBlocksCached*512)) { printf("[x] read_emmc: read failed for index cache for outBufTmpCached: %d\n", res); perror("read failed: "); return -1; } memcpy(outBuf, outBufTmpCached, nBlocksCached*512); //Assume offset % 512 == 0 } else printf("[+] read_emmc: no data to get from cache\n"); /* Get data from HBOOT */ if (nBlocksHBOOT > 0) { printf("[+] read_emmc: getting data from HBOOT\n"); printf("[+] read_emmc: userdata start at sector: %d\n", userdata_sector); printf("[+] read_emmc: offset in userdata: %d\n", startBlockHBOOT); // Prepare temporary buffer char* outBufTmpHBOOT = (char*)malloc(nBlocksHBOOT*512*sizeof(char)); if (!outBufTmpHBOOT) { printf("[x] read_emmc: malloc failed\n"); return -1; } /* Prepare command and send it: hboot>read_emmc command format: read_emmc [start] [#blocks] [#blocks/read] [show] */ sprintf(input, "read_emmc %u %d %d 1\r\n", userdata_sector+startBlockHBOOT, nBlocksHBOOT, nBlocksHBOOTPerRead); printf("[+] read_emmc: writing (%d): \"%s\"\n", strlen(input), input); s->write(input, strlen(input)); //printf("[+] reading now...\n"); // Process all lines // Loop until we read all the bytes corresponding to our sent command (more than we need actually ;) while (nBytesReadHBOOT < nBlocksHBOOT*512) { nBytesReadPerLineHBOOT = 0; printf("%d - ", nBytesReadHBOOT); memset(output, 0, LINE_SIZE); res = s->getline(output, LINE_SIZE); //sleep(1); // Skip what is not our data and sent randomly from HBOOT if (!strcmp(output, "\r")) { //printf("[+] read_emmc: output=\"\\r\", continuing\n"); continue; } else if (!strcmp(output, "hboot>\r")) { //printf("[+] read_emmc: output=\"hboot>\\r\", continuing\n"); continue; } else if (strstr(output, "reading sector")) { //printf("[+] read_emmc: output=\"reading sector...\", continuing\n"); continue; } else if (strstr(output, "read sector")) { //printf("[+] read_emmc: output=\"read sector...\", continuing\n"); continue; } else if (strstr(output, "hboot>read_emmc ")) { //printf("[+] read_emmc: output=\"hboot>read_emmc\", continuing\n"); continue; } else if (strstr(output, "hboot>") && (strstr(output, "emmc")||strstr(output, "read"))) { //printf("[+] read_emmc: output=\"hboot>red_emmc\", continuing\n"); //missing letters sometime... continue; } else if (strstr(output, "read_emmc ")) { //printf("[+] read_emmc: output=\"read_emmc\", continuing\n"); continue; } // We have valid bytes, let's parse them char *pch = output; char *endptr = NULL; long h = 0; while (pch && strlen(pch) > 0 && strcmp(pch, "\r") && strcmp(pch, " \r")) { //printf("(%x)", pch); errno = 0; /* To distinguish success/failure after call */ h = strtol(pch, &endptr, 16); //printf("-> [%x, %x, %x]\n", h, pch, endptr); if ((errno == ERANGE && (h == LONG_MAX || h == LONG_MIN)) || (errno != 0 && h == 0)) { printf("[+] read_emmc: strtol failed\n"); if (pch) hex_dump(pch, strlen(pch)); break; } if (endptr == pch) { printf("[+] read_emmc: no digits were found\n"); if (pch) hex_dump(pch, strlen(pch)); break; } if (*endptr) { pch = endptr+1; } outBufTmpHBOOT[nBytesReadHBOOT+nBytesReadPerLineHBOOT] = (char)h; nBytesReadPerLineHBOOT += 1; } /* It may happen we do not receive 16 bytes in a line, do not know why this is happening so let's fake it for now. [+] read_emmc: strlen(output)=29 [0000] 30 20 30 20 30 20 30 20 30 20 30 20 30 20 30 20 0.0.0.0. 0.0.0.0. [0010] 30 20 30 20 30 20 30 20 30 20 30 20 0D 0.0.0.0. 0.0.. */ if (nBytesReadPerLineHBOOT > 0 && nBytesReadPerLineHBOOT < 16) { printf("[+] read_emmc: read %d bytes for line\n", nBytesReadPerLineHBOOT); printf("[+] read_emmc: strlen(output)=%d\n", strlen(output)); hex_dump(output, strlen(output)); printf("[+] read_emmc: fake it for now\n"); //so we fake it with 0x00 bytes nBytesReadPerLineHBOOT = 16; } else if (nBytesReadPerLineHBOOT == 0) { printf("[+] read_emmc: nBytesReadPerLineHBOOT == 0\n"); printf("[+] read_emmc: strlen(output)=%d\n", strlen(output)); hex_dump(output, strlen(output)); } nBytesReadHBOOT += nBytesReadPerLineHBOOT; } // We have processed all lines from here //Read remaining data from serial bus so we are ready to go for next write/read prompt = NULL; loop = 0; printf("[+] read_emmc: reading remaining data...\n"); while (!prompt) { memset(output, 0, LINE_SIZE); s->getline(output, LINE_SIZE); //sleep(1); prompt = strstr(output, "hboot>"); //printf("[+] read_emmc: skipping strlen(output)=%d\n", strlen(output)); hex_dump(output, strlen(output)); loop++; } //printf("[+] read_emmc: loop %d time(s)\n", loop); //memcpy(outBuf, outBufTmpHBOOT + (offset % 512), nBytes); printf("[+] read_emmc: copying data from outBufTmpHBOOT=%lx to outBuf=%lx now\n", (unsigned long)outBufTmpHBOOT, (unsigned long)outBuf); printf("[+] read_emmc: memcpy(%lx, %lx, %d)\n", (unsigned long)(outBuf + nBlocksCached*512), (unsigned long)outBufTmpHBOOT, nBlocksHBOOT*512); memcpy(outBuf + nBlocksCached*512, outBufTmpHBOOT, nBlocksHBOOT*512); //Assume offset % 512 == 0 //hex_dump(outBuf, 0x500); // Write all read sectors into cache (do not check if we already saved them, do not worry) if (lseek(cache_fd, startBlockHBOOT*512, SEEK_SET) != startBlockHBOOT*512) { //We support userdata partition size < 2^32 ~ 4GB printf("[x] usb_read: can not seek in cache file: %s\n", cache_filename); return -1; } if (write(cache_fd, outBufTmpHBOOT, nBlocksHBOOT*512) != (int)(nBlocksHBOOT*512)) { printf("[x] read_emmc: write failed for cache\n"); return -1; } // And update index cache of already saved sectors (same no-check here) // For now, a byte holds 1 index value: 0x1 says one sector in cache is saved, 0x0 says it is not saved yet if (lseek(indexcache_fd, startBlockHBOOT, SEEK_SET) != startBlockHBOOT) { printf("[x] usb_read: can not seek in index cache file for writing: %s\n", indexcache_filename); return -1; } char* ones = (char*)malloc(nBlocksHBOOT*sizeof(char)); if (!ones) { printf("[x] read_emmc: malloc failed for updating index cache\n"); return -1; } memset(ones, 0x01, nBlocksHBOOT*sizeof(char)); if (write(indexcache_fd, ones, nBlocksHBOOT) != (int)nBlocksHBOOT) { printf("[x] read_emmc: write failed for index cache\n"); return -1; } if (ones) { free(ones); ones = NULL; } if (outBufTmpHBOOT) { free(outBufTmpHBOOT); outBufTmpHBOOT = NULL; } } /* End of getting data from HBOOT */ else printf("[+] read_emmc: no data to get from HBOOT\n"); // Free resources close(cache_fd); close(indexcache_fd); //printf("[+] read_emmc end: %d\n", nBytes); return nBytes; }