/* * gpsd for motorola a780 0.1 * * init code reverse engineered by mack * protocol re'd by floe * server part implemented by lurker * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * */ #include #include #include #include #include #include #include #include #define LAPI_MSG_INIT "Z\1\0\0d\0\4\0\1\0\0\0" #define LAPI_MSG_OPEN "Z\1\0\0f\0\4\0" /* + 4 bytes globalId */ #define LAPI_MSG_START "Z\1\0\0j\0 \0\0\0\0\0\2\0\0\0\226\0\0\0\226\0\0\0\210\23\0\0\1\0\0\0\1\0\0\0\1\0\0\0" #define LAPI_MSG_STOP "Z\1\0\0l\0\0\0" #define LAPI_MSG_CLOSE "Z\1\0\0h\0\0\0" typedef struct { unsigned char padding[6]; // necessary due to the fucking 8 byte double alignment of ARM architecture unsigned int pid; unsigned short msgid; unsigned short msglen; unsigned char sessid; unsigned char transid; double longitude; double latitude; double accuracy; unsigned long long int timestamp; // milliseconds since the epoch (1 Jan 1970 00:00:00) (in UTC) double altitude; double speed; double heading; double bearing; // always -256.0 double horizontal_accuracy; // always equal to accuracy double vertical_accuracy; double course; // always equal to heading int locmethod; int cause; unsigned long long int ageoffix; // also a millisecond counter } lapi_gps_rec; int run = 1; int connected; void error(char *msg) { perror(msg); exit(1); } void quitsignal() { run = 0; } int wait_for_data(int socket1, int socket2); int main(int argc, char *argv[]) { int ctrlsock, datasock, servlen, netsock, servsock; unsigned int i, j, k, l, n; struct sockaddr_un serv_addr; struct sockaddr_in sock_name; unsigned char buffer[256]; unsigned char command[8192]; unsigned char answer[8192]; lapi_gps_rec* record = (lapi_gps_rec*)&buffer; double alt, lat, lon, speed, course; signal( SIGHUP, quitsignal ); signal( SIGINT, quitsignal ); signal( SIGQUIT, quitsignal ); signal( SIGTERM, quitsignal ); fprintf( stderr, "GPSD for Motorola A780 by lurker , based on gpsdump by Floe \n" ); if (argc != 2) { fprintf(stderr, "Usage: %s portnumber\n", argv[0]); exit(1); } if ((netsock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { perror("socket"); exit(1); } memset(&sock_name, 0, sizeof(sock_name)); sock_name.sin_family = AF_INET; sock_name.sin_port = htons(atoi(argv[1])); sock_name.sin_addr.s_addr = INADDR_ANY; if (bind(netsock, (struct sockaddr *)&sock_name, sizeof(sock_name))) { perror("bind"); close(netsock); exit(1); } if (listen(netsock, 5)) { perror("listen"); close(netsock); exit(1); } bzero( (char*)&serv_addr, sizeof(serv_addr) ); serv_addr.sun_family = AF_UNIX; strcpy( serv_addr.sun_path, "/tmp/lapisock" ); servlen = strlen(serv_addr.sun_path) + sizeof(serv_addr.sun_family); if ((ctrlsock = socket( AF_UNIX, SOCK_STREAM, 0 )) < 0) error("Creating control socket"); if ((datasock = socket( AF_UNIX, SOCK_STREAM, 0 )) < 0) error("Creating data socket"); if (connect( ctrlsock, (struct sockaddr*)&serv_addr, servlen ) < 0) error("Connecting control socket"); if (connect( datasock, (struct sockaddr*)&serv_addr, servlen ) < 0) error("Connecting data socket"); fprintf( stderr, "Opening LAPI.." ); fflush(0); write( ctrlsock, LAPI_MSG_INIT, sizeof(LAPI_MSG_INIT)-1 ); read( ctrlsock, buffer, sizeof(buffer) ); memcpy( buffer, LAPI_MSG_OPEN, sizeof(LAPI_MSG_OPEN)-1 ); write( datasock, buffer, sizeof(LAPI_MSG_OPEN)-1+4 ); read( datasock, buffer, sizeof(buffer) ); write( ctrlsock, LAPI_MSG_START, sizeof(LAPI_MSG_START)-1 ); fprintf( stderr, "done.\n" ); while (run) { if (wait_for_data(netsock, datasock) == 0) { n = read(datasock, buffer+6, sizeof(buffer)-6); } else if (wait_for_data(netsock, datasock) == 1) { j = sizeof(sock_name); if ((servsock = accept(netsock, (struct sockaddr *)&sock_name, &j)) < 0) { perror("accept"); close(netsock); exit(1); } connected = 1; do { if (wait_for_data(servsock, datasock) == 0) { n = read(datasock, buffer+6, sizeof(buffer)-6); alt = record->altitude; lat = record->latitude; lon = record->longitude; speed = record->speed; course = record->course; } else if (wait_for_data(servsock, datasock) == 1) { if ((k = recv(servsock, command, 8192, 0)) < 0) { connected = 0; perror("recv"); } else { if (k == 0) { connected = 0; } else { i = 0; l = 0; sprintf(&answer[l], "GPSD"); l = 4; while (command[i] != '\n') { switch (command[i]) { case 'a': case 'A': l = l + sprintf(&answer[l], ",A=%f", alt); break; case 'b': case 'B': l = l + sprintf(&answer[l], ",B=38400 8 N 1"); break; case 'd': case 'D': break; case 'h': case 'H': break; case 'p': case 'P': l = l + sprintf(&answer[l], ",P=%f %f", lat, lon); break; case 's': case 'S': l = l + sprintf(&answer[l], ",S=1"); break; case 't': case 'T': l = l + sprintf(&answer[l], ",T=%f", course); break; case 'v': case 'V': l = l + sprintf(&answer[l], ",V=%f", speed); break; case 'x': case 'X': l = l + sprintf(&answer[l], ",X=1"); break; default: break; } i++; } l = l + sprintf(&answer[l], "\r\n"); send(servsock, answer, l, 0); } } } } while (connected && run); shutdown(servsock, 2); close(servsock); } } fprintf( stderr, "Releasing LAPI.." ); fflush( 0 ); write( ctrlsock, LAPI_MSG_STOP, sizeof(LAPI_MSG_STOP)-1 ); //read( ctrlsock, buffer, sizeof(buffer) ); write( ctrlsock, LAPI_MSG_CLOSE, sizeof(LAPI_MSG_CLOSE)-1 ); read( ctrlsock, buffer, sizeof(buffer) ); close(netsock); fprintf(stderr, "done.\n"); return 0; } int wait_for_data(int socket1, int socket2) { int i; fd_set fds; FD_ZERO(&fds); FD_SET(socket1, &fds); FD_SET(socket2, &fds); if (select(FD_SETSIZE, &fds, NULL, NULL, NULL) < 0) { i = -1; } if (FD_ISSET(socket2, &fds)) { i = 0; } if (FD_ISSET(socket1, &fds)) { i = 1; } return i; }