/* this is the state of our connection to the qemu_gpsd daemon */
typedef struct {
int init;
int fd; // UART5 FD
#if GPS_GPIO_INCLUDE
int fdGps; // GPS_GPIO FD
#endif
GpsCallbacks callbacks;
pthread_t thread;
int control[2];
char device[32];
sem_t fix_sem;
// QemuChannel channel; // We are not use QEMU
} GpsState;
/*****************************************************************/
/*****************************************************************/
/***** *****/
/***** I N T E R F A C E *****/
/***** *****/
/*****************************************************************/
/*****************************************************************/
static int
sirf_gps_init(GpsCallbacks* callbacks) //初始化,call gps_state_init(s)
{
D("%s", __FUNCTION__);
GpsState* s = _gps_state;
s->callbacks = *callbacks;
if (!s->init){
while(1){ //loop to make sure open fd correctly.
if(1 == gps_state_init(s))
break;
usleep(1000*1000);
}
}
if (s->fd < 0)
return -1;
/*s->callbacks = *callbacks;*/
D("%s: called", __FUNCTION__);
return 0;
}
static void
sirf_gps_cleanup(void)
{
GpsState* s = _gps_state;
if (s->init)
gps_state_done(s);
D("%s: called", __FUNCTION__);
}
static int
sirf_gps_start() // app will call this.
{
GpsState* s = _gps_state;
if (!s->init) {
D("%s: called with uninitialized state !!", __FUNCTION__);
return -1;
}
D("%s: called", __FUNCTION__);
gps_state_start(s); //gps start
return 0;
}
static int
sirf_gps_stop() //app quit will call this, system sleep will call this too.
{
GpsState* s = _gps_state;
if (!s->init) {
D("%s: called with uninitialized state !!", __FUNCTION__);
return -1;
}
D("%s: called", __FUNCTION__);
gps_state_stop(s);
return 0;
}
static int
sirf_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty)
{
D("%s: called", __FUNCTION__);
return 0;
}
static int
sirf_gps_inject_location(double latitude, double longitude, float accuracy)
{
D("%s: called", __FUNCTION__);
return 0;
}
static void
sirf_gps_delete_aiding_data(GpsAidingData flags)
{
D("%s: called", __FUNCTION__);
}
static int sirf_gps_set_position_mode(GpsPositionMode mode, int fix_frequency)
{
D("%s: called", __FUNCTION__);
return 0;
}
static const void*
sirf_gps_get_extension(const char* name)
{
return NULL;
}
static const GpsInterface StarViewGpsInterface = {
.size = sizeof(GpsInterface),
.init = sirf_gps_init,
.start = sirf_gps_start,
.stop = sirf_gps_stop,
.cleanup = sirf_gps_cleanup,
.inject_time = sirf_gps_inject_time,
.inject_location = sirf_gps_inject_location,
.delete_aiding_data = sirf_gps_delete_aiding_data,
.set_position_mode = sirf_gps_set_position_mode,
.get_extension = sirf_gps_get_extension,
};
const GpsInterface* gps_get_hardware_interface()
{
D("%s: called\n", __FUNCTION__);
return &StarViewGpsInterface;
}
static int open_gps(const struct hw_module_t* module, char const* name, struct hw_device_t** device)
{
D("%s %s", __FUNCTION__,name);
struct gps_device_t *dev = malloc(sizeof(struct gps_device_t));
memset(dev, 0, sizeof(*dev));
dev->common.tag = HARDWARE_DEVICE_TAG;
dev->common.version = 0;
dev->common.module = (struct hw_module_t*)module;
dev->get_gps_interface = gps_get_hardware_interface;
*device = (struct hw_device_t*)dev;
return 0;
}
static struct hw_module_methods_t gps_module_methods = {
.open = open_gps
};
const struct hw_module_t HAL_MODULE_INFO_SYM = {
.tag = HARDWARE_MODULE_TAG,
.version_major = 2011,
.version_minor = 2,
.id = GPS_HARDWARE_MODULE_ID,
.name = "Navisys GAS4R module & u-blox 5 chip",
.author = "Wind River",
.methods = &gps_module_methods,
};
/* this is the main thread, it waits for commands from gps_state_start/stop and,
* when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
* that must be parsed to be converted into GPS fixes sent to the framework
*/
static void*
gps_state_thread( void* arg ) //create a new thread for gps
{
GpsState* state = (GpsState*) arg;
NmeaReader reader[1];
int epoll_fd = epoll_create(2);
int started = 0;
int gps_fd = state->fd;
int control_fd = state->control[1];
nmea_reader_init( reader );
/* register control file descriptors for polling */
epoll_register( epoll_fd, control_fd );
epoll_register( epoll_fd, gps_fd );
/* now loop */
for (;;) {
struct epoll_event events[2];
int ne, nevents;
nevents = epoll_wait( epoll_fd, events, 2, -1 );
if (nevents < 0) {
if (errno != EINTR)
LOGE("epoll_wait() unexpected error: %s", strerror(errno));
continue;
}
for (ne = 0; ne < nevents; ne++) {
if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
LOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
goto Exit;
}
if ((events[ne].events & EPOLLIN) != 0) {
int fd = events[ne].data.fd;
if (fd == control_fd)
{
char cmd = 255;
int ret;
do {
ret = read( fd, &cmd, 1 );
} while (ret < 0 && errno == EINTR);
if (cmd == CMD_QUIT) {
D("gps thread quitting on demand");
goto Exit;
}
else if (cmd == CMD_START) {
if (!started) {
D("gps thread starting location_cb=%p", state->callbacks.location_cb);
started = 1;
#ifdef Svpnd_Version
/*nmea_reader_set_callback( reader, state->callbacks);*/
reader->callback.location_cb = state->callbacks.location_cb;
reader->callback.status_cb = state->callbacks.status_cb;
reader->callback.sv_status_cb = state->callbacks.sv_status_cb;
if (/*cb != NULL &&*/ reader->fix.flags != 0) {
D("%s: sending latest fix to new callback", __FUNCTION__);
reader->callback.location_cb( &reader->fix );
reader->fix.flags = 0;
}
reader->gps_status.status=GPS_STATUS_SESSION_BEGIN;
state->callbacks.status_cb(&(reader->gps_status));
#else
/*nmea_reader_set_callback( reader, state->callbacks.location_cb );*/
reader->callback = state->callbacks.location_cb;
if (cb != NULL && reader->fix.flags != 0) {
D("%s: sending latest fix to new callback", __FUNCTION__);
reader->callback( &reader->fix );
reader->fix.flags = 0;
}
#endif
}
}
else if (cmd == CMD_STOP) {
if (started) {
D("gps thread stopping");
started = 0;
#ifndef Svpnd_Version
/*nmea_reader_set_callback( reader, NULL );*/
reader->gps_status.status=GPS_STATUS_SESSION_END;
state->callbacks.status_cb(&(reader->gps_status));
reader->callback.location_cb = NULL;
reader->callback.status_cb = NULL;
reader->callback.sv_status_cb = NULL;
if (/*cb != NULL &&*/ reader->fix.flags != 0) {
D("%s: sending latest fix to new callback", __FUNCTION__);
reader->callback.location_cb( &reader->fix );
reader->fix.flags = 0;
}
#endif
}
}
}
else if (fd == gps_fd)
{
char buff[32];
/* D("gps fd event"); */
for (;;) {
int nn, ret;
ret = read( fd, buff, sizeof(buff) );
if (ret < 0) {
if (errno == EINTR)
continue;
if (errno != EWOULDBLOCK)
LOGE("error while reading from gps daemon socket: %s:", strerror(errno));
break;
}
#ifdef Svpnd_Version
nmea_reader_add( reader, buff,ret);
#else
for (nn = 0; nn < ret; nn++)
nmea_reader_addc( reader, buff[nn] );
#endif
}
/* D("gps fd event end"); */
}
else
{
LOGE("epoll_wait() returned unkown fd %d ?", fd);
}
}
}
}
Exit:
return NULL;
}
static int
gps_channel_open(TccChannel* channel,const char* name,int mode)
{
int fd = -1;
if (!channel->is_inited)
{
channel->is_inited = 1;
}
LOGD("channel->name: %s name: %s\n",channel->device,name);
do
{
fd = open(name, mode);
} while (fd < 0 && errno == EINTR);
/* disable ECHO on serial lines */
if (fd >= 0 && channel->is_tty) {
struct termios ios;
tcgetattr( fd, &ios );
ios.c_lflag = 0; /* disable ECHO, ICANON, etc... */
tcsetattr( fd, TCSANOW, &ios );
}
/* set baudrate */
struct termios ios;
tcgetattr( fd, &ios );
cfsetispeed(&ios,B4800);
cfsetospeed(&ios,B4800);
tcsetattr( fd, TCSANOW, &ios );
return fd;
}
static void
gps_state_done( GpsState* s )
{
/* tell the thread to quit, and wait for it */
char cmd = CMD_QUIT;
void* dummy;
int ret;
do {
ret=write( s->control[0], &cmd, 1 );
} while (ret < 0 && errno == EINTR);
if (ret != 1)
D("%s: could not send CMD_QUIT command: ret=%d: %s", __FUNCTION__, ret, strerror(errno));
pthread_join(s->thread, &dummy);
/* close the control socket pair */
close( s->control[0] ); s->control[0] = -1;
close( s->control[1] ); s->control[1] = -1;
/* close connection to the QEMU GPS daemon */
close( s->fd ); s->fd = -1; /* UART 5 */
sem_destroy(&s->fix_sem); /*release the semaphore */
#if GPS_GPIO_INCLUDE
close( s->fdGps ); s->fdGps = -1; /* GPS_GPIO */
#endif
s->init = 0;
}
static void
gps_state_stop( GpsState* s )
{
char cmd = CMD_STOP;
int ret;
FILE *fp;
char state_value;
do { ret=write( s->control[0], &cmd, 1 ); }
while (ret < 0 && errno == EINTR);
if (ret != 1)
D("%s: could not send CMD_STOP command: ret=%d: %s",
__FUNCTION__, ret, strerror(errno));
close( s->fd );
sem_destroy(&s->fix_sem);
usleep(100*1000);
#if GPS_GPIO_INCLUDE
/*set on_off is off to close gps */
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
state_value = '0';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Read error.");
return;
}
D("Read data = %c", state_value);
fclose(fp);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
state_value = '1';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("----- Read error.");
return;
}
D("Read data 2= %c", state_value);
fclose(fp);
#endif
}
static int
gps_state_init( GpsState* state ) // gps init
{
struct termios termios;
char key[PROPERTY_KEY_MAX];
char prop[PROPERTY_VALUE_MAX];
FILE *fp;
char state_value;
int version;
int ret, i;
struct pollfd Event;
char buf[1];
state->init = 1;
state->control[0] = -1;
state->control[1] = -1;
state->fd = -1;
#if GPS_GPIO_INCLUDE
state->fdGps = -1;
#endif
strcpy(&state->device,GPS_CHANNEL_NAME);
/* open ttyUSB0 */
state->fd = open(GPS_CHANNEL_NAME, O_RDWR | O_NOCTTY);
if (state->fd < 0) {
D("Star View : no gps Hardware detected");
return -1 ;
}
tcflush(state->fd, TCIOFLUSH);
tcgetattr(state->fd, &termios);
termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP
| INLCR | IGNCR | ICRNL | IXON);
termios.c_oflag &= ~OPOST;
termios.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
termios.c_cflag &= ~(CSIZE | PARENB);
termios.c_cflag |= CS8;
#if GPS_UART_RTS_CTS_FLOW_CONTROL_ENABLE
termios.c_cflag |= CRTSCTS;
#else
termios.c_cflag &= ~CRTSCTS;
#endif
tcsetattr(state->fd, TCSANOW, &termios);
tcflush(state->fd, TCIOFLUSH);
tcsetattr(state->fd, TCSANOW, &termios);
tcflush(state->fd, TCIOFLUSH);
tcflush(state->fd, TCIOFLUSH);
cfsetospeed(&termios, B4800);
cfsetispeed(&termios, B4800);
tcsetattr(state->fd, TCSANOW, &termios);
tcflush(state->fd, TCIOFLUSH);
usleep(1000*1000);
memset (&Event, 0, sizeof(Event));
Event.fd = state->fd;
Event.events = POLLIN | POLLERR;
for (i = 0; i < 5; i++ ) {
ret = poll (&Event, 1, 1000);
if (ret < 0) {
D(" poll error");
exit (EXIT_FAILURE);
}
if (Event.revents & POLLIN) {
D("can read data. \n");
if(read(state->fd, buf,1)){
D("buf[0] = %c", buf[0]);
if(NULL == buf[0]){
D("continue");
continue;
}
}
break;
}
if (ret == 0) {
D("no data in 1 seconds.\n");
/*set on_off is off to close gps */
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
state_value = '0';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return -1;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Read error.");
return -1;
}
D("Read data = %c", state_value);
fclose(fp);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
state_value = '1';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return -1;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("----- Read error.");
return -1;
}
D("Read data 2= %c", state_value);
fclose(fp);
}
if(4 == i ){
close( state->fd );
return 0;
}
}
usleep(1000*1000);
usleep(1000*1000);
if(SendPatch(state->fd)){
D("Send Patch OK!");
}
else{
D("Send Patch failed!");
}
/*set GPS on_off is off to close gps */
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
state_value = '0';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return -1;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Read error.");
return -1;
}
D("Read data = %c", state_value);
fclose(fp);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
state_value = '1';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return -1;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return -1;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("----- Read error.");
return -1;
}
D("Read data 2= %c", state_value);
fclose(fp);
close( state->fd );
return 1;
Fail:
gps_state_done( state );
}
static void
gps_state_start( GpsState* s ) //start gps
{
char cmd = CMD_START;
int ret;
FILE *fp;
char state_value;
struct termios termios;
struct pollfd Event;
/* s->init = 1;*/
s->fd = -1;
strcpy(&s->device,GPS_CHANNEL_NAME);
s->fd = open(GPS_CHANNEL_NAME, O_RDWR | O_NOCTTY); /* open ttyUSB0 */
if (s->fd < 0) {
D("Star View : no gps Hardware detected");
return;
}
tcflush(s->fd, TCIOFLUSH);
tcgetattr(s->fd, &termios);
termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP
| INLCR | IGNCR | ICRNL | IXON);
termios.c_oflag &= ~OPOST;
termios.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
termios.c_cflag &= ~(CSIZE | PARENB);
termios.c_cflag |= CS8;
#if GPS_UART_RTS_CTS_FLOW_CONTROL_ENABLE
termios.c_cflag |= CRTSCTS;
#else
termios.c_cflag &= ~CRTSCTS;
#endif
tcsetattr(s->fd, TCSANOW, &termios);
tcflush(s->fd, TCIOFLUSH);
tcsetattr(s->fd, TCSANOW, &termios);
tcflush(s->fd, TCIOFLUSH);
tcflush(s->fd, TCIOFLUSH);
cfsetospeed(&termios, B4800);
cfsetispeed(&termios, B4800);
tcsetattr(s->fd, TCSANOW, &termios);
tcflush(s->fd, TCIOFLUSH);
if (sem_init(&s->fix_sem, 0, 1) != 0) {
D("gps semaphore initialization failed! errno = %d", errno);
goto Fail;
}
if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, s->control ) < 0 ) {
LOGE("could not create thread control socket pair: %s", strerror(errno));
goto Fail;
}
if ((s->thread = s->callbacks.create_thread_cb("myGPS",gps_state_thread,s))==NULL) {
D("could not create gps thread: %s", strerror(errno));
goto Fail;
}
usleep(100*1000);
usleep(100*1000);
do { ret=write( s->control[0], &cmd, 1 ); }
while (ret < 0 && errno == EINTR);
if (ret != 1)
D("%s: could not send CMD_START command: ret=%d: %s",
__FUNCTION__, ret, strerror(errno));
usleep(100*1000);
memset (&Event, 0, sizeof(Event));
Event.fd = s->fd;
Event.events = POLLIN | POLLERR;
while(1) {
ret = poll (&Event, 1, 1000);
if (ret < 0) {
D("poll");
exit (EXIT_FAILURE);
}
if (Event.revents & POLLIN) {
D("can read data. \n");
break;
}
if (ret == 0) {
D("read no data in 1 seconds.\n");
/*set on_off is off to close gps */
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
state_value = '0';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Read error.");
return;
}
D("Read data = %c", state_value);
fclose(fp);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "wb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
state_value = '1';
if( fwrite(&state_value, sizeof(state_value), 1, fp) != 1) {
D("Write error.\n");
return;
}
fclose(fp);
usleep(100*1000);
if((fp = fopen("/sys/class/rfkill/rfkill2/state", "rb")) == NULL) {
D("Cannot open /sys/class/rfkill/rfkill2/state.");
return;
}
if(fread(&state_value, sizeof(state_value), 1, fp) != 1) {
D("----- Read error.");
return;
}
D("Read data 2= %c", state_value);
fclose(fp);
break;
}
}
return 1;
Fail:
gps_state_done( s );
}