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);
}
}
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;
}
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;
}
/**
* 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;
}