Beispiel #1
0
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")
}
Beispiel #4
0
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);
    }
}
Beispiel #5
0
/**
 * 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;
}
Beispiel #6
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;
}
Beispiel #9
0
/**
 * 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;
}