WebSVN – public – Blame – /sportwatcher/trunk/libgant/gant.cpp

Rev	Author	Line No.	Line
230	andreas	1	`/***************************************************************************`
		2	`* Copyright (C) 2007 to 2009 by Andreas Theofilu *`
		3	`* andreas@theosys.at *`
		4	`* *`
		5	`* This program is free software; you can redistribute it and/or modify *`
		6	`* it under the terms of the GNU General Public License as published by *`
		7	`* the Free Software Foundation version 3 of the License. *`
		8	`* *`
		9	`* This program is distributed in the hope that it will be useful, *`
		10	`* but WITHOUT ANY WARRANTY; without even the implied warranty of *`
		11	`* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *`
		12	`* GNU General Public License for more details. *`
		13	`* *`
		14	`* You should have received a copy of the GNU General Public License *`
		15	`* along with this program; if not, write to the *`
		16	`* Free Software Foundation, Inc., *`
		17	`* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *`
		18	`***************************************************************************/`
		19
		20	`#include "gant.h"`
		21	`#include <sys/select.h>`
		22	`#include <unistd.h>`
		23	`#include <sys/types.h>`
		24	`#include <sys/stat.h>`
		25	`#include <fcntl.h>`
		26	`#include <stdio.h>`
		27	`#include <string.h>`
		28	`#include <termios.h>`
		29	`#include <stdlib.h>`
		30	`#include <assert.h>`
		31	`#include <kmessagebox.h>`
		32	`#include <klocale.h>`
		33
		34	`static char G50_KEY[] = "A8A423B9F55E63C1"; // garmin network key`
		35
		36	`static char sendack1[] = "4402320499990000";`
		37	`// 44 message from pc?`
		38	`// ..02 message type`
		39	`// ....32 don't know`
		40	`// ......04 don't know`
		41	`// ........9999 my id`
		42	`// ............0000 maybe id is 4 bytes`
		43	`static char sendack2[] = "4404010099990000";`
		44	`// ..04 message type`
		45	`// ....01 don't know`
		46	`// ......00 don't know`
		47	`static char sendack3[] = "4406010000000000";`
		48	`// ..06 message type`
		49	`// ....01 don't know`
		50	`static char sendack4[] = "4403000000000000";`
		51	`// ..03 message type, go to idle`
		52	`// ....01 don't know`
		53
		54	`ant::ant ()`
		55	`{`
		56	`qFile = false;`
		57	`commenabled = 1;`
		58	`rfn = cfn = 0;`
		59	`}`
		60
		61	`ant::~ant ()`
		62	`{`
		63	`if (qFile)`
		64	`device.close ();`
		65
		66	`qFile = false;`
		67	`rfn = cfn = 0;`
		68	`}`
		69
		70	`/*`
		71	`* Set the device and store it inside the class`
		72	`*/`
		73	`bool ant::setDevice (const QFile &fl)`
		74	`{`
		75	`if (qFile)`
		76	`{`
		77	`device.close();`
		78	`qFile = false;`
		79	`}`
		80
		81	`device.setName (fl.name ());`
		82
		83	`if (!device.exists ())`
		84	`{`
		85	`KMessageBox::error (0, i18n("Device %1 does not exist!").arg(fl.name()));`
		86	`return false;`
		87	`}`
		88
		89	`if (!device.open (IO_Raw \| IO_ReadWrite))`
		90	`{`
		91	`KMessageBox::error (0, i18n("Error opening device %1.").arg(fl.name()),`
		92	`device.errorString());`
		93	`return false;`
		94	`}`
		95
		96	`qFile = true;`
		97	`return true;`
		98	`}`
		99
		100	`bool ant::setDevice (const QString &fl)`
		101	`{`
		102	`QFile f;`
		103
		104	`if (qFile)`
		105	`{`
		106	`device.close ();`
		107	`qFile = false;`
		108	`}`
		109
		110	`f.setName (fl);`
		111	`return setDevice (f);`
		112	`}`
		113
		114	`/*`
		115	`* Send a message to the opened device.`
		116	`*/`
		117	`bool ant::msg_send(uchar mesg, uchar *inbuf, uchar len)`
		118	`{`
		119	`uchar buf[MAXMSG];`
		120	`ssize_t nw;`
		121	`int i;`
		122	`uchar chk = MESG_TX_SYNC;`
		123
		124	`buf[0] = MESG_TX_SYNC;`
		125	`buf[1] = len;`
		126	`chk ^= len;`
		127	`buf[2] = mesg;`
		128	`chk ^= mesg;`
		129
		130	`for (i = 0; i < len; i++)`
		131	`{`
		132	`buf[3+i] = inbuf[i];`
		133	`chk ^= inbuf[i];`
		134	`}`
		135
		136	`buf[3+i] = chk;`
		137	`usleep (10 * 1000);`
		138
		139	`if ((4 + i) != (nw = write(device.handle(), buf, 4 + i)))`
		140	`{`
		141	`KMessageBox::error (0, i18n("Error writing to device %1").arg(device.name()));`
		142	`return false;`
		143	`}`
		144
		145	`return true;`
		146	`}`
		147
		148	`// two argument send`
		149	`bool ant::msg_send(uchar mesg, uchar data1, uchar data2)`
		150	`{`
		151	`uchar buf[2];`
		152
		153	`buf[0] = data1;`
		154	`buf[1] = data2;`
		155	`return msg_send(mesg, buf, 2);`
		156	`}`
		157
		158	`// three argument send`
		159	`bool ant::msg_send(uchar mesg, uchar data1, uchar data2, uchar data3)`
		160	`{`
		161	`uchar buf[3];`
		162
		163	`buf[0] = data1;`
		164	`buf[1] = data2;`
		165	`buf[2] = data3;`
		166	`return msg_send(mesg, buf, 3);`
		167	`}`
		168
		169	`bool ant::commfn ()`
		170	`{`
		171	`fd_set readfds, writefds, exceptfds;`
		172	`int ready, fd;`
		173	`struct timeval to;`
		174
		175	`fd = device.handle ();`
		176
		177	`for(;;)`
		178	`{`
		179	`FD_ZERO(&readfds);`
		180	`FD_ZERO(&writefds);`
		181	`FD_ZERO(&exceptfds);`
		182	`FD_SET(fd, &readfds);`
		183	`to.tv_sec = 1;`
		184	`to.tv_usec = 0;`
		185	`ready = select(fd+1, &readfds, &writefds, &exceptfds, &to);`
		186
		187	`if (ready)`
		188	`{`
		189	`if (!get_data(fd))`
		190	`return false;`
		191	`}`
		192	`}`
		193
		194	`return true;`
		195	`}`
		196
		197	`bool ant::get_data(int fd)`
		198	`{`
		199	`static uchar buf[500];`
		200	`static int bufc = 0;`
		201	`int nr;`
		202	`int dlen;`
		203	`int i;`
		204	`int j;`
		205	`unsigned char chk = 0;`
		206	`uchar event;`
		207	`int found;`
		208	`int srch;`
		209	`int next;`
		210
		211	`nr = read(fd, buf+bufc, 20);`
		212
		213	`if (nr > 0)`
		214	`bufc += nr;`
		215	`else`
		216	`return true;`
		217
		218	`if (bufc > 300)`
		219	`{`
		220	`KMessageBox::error (0, i18n(">>buf<< too long!"));`
		221	`return false;`
		222	`}`
		223
		224	`// some data in buf`
		225	`// search for possible valid messages`
		226	`srch = 0;`
		227
		228	`while (srch < bufc)`
		229	`{`
		230	`found = 0;`
		231
		232	`for (i = srch; i < bufc; i++)`
		233	`{`
		234	`if (buf[i] == MESG_TX_SYNC)`
		235	`{`
		236	`if (i+1 < bufc && buf[i+1] >= 1 && buf[i+1] <= 13)`
		237	`{`
		238	`dlen = buf[i+1];`
		239
		240	`if ((i + 3 + dlen) < bufc)`
		241	`{`
		242	`chk = 0;`
		243
		244	`for (j = i; j <= i+3+dlen; j++)`
		245	`chk ^= buf[j];`
		246
		247	`if (0 == chk)`
		248	`{`
		249	`found = 1; // got a valid message`
		250	`break;`
		251	`}`
		252	`else`
		253	`fprintf (stderr, "bad chk %02x\n", chk);`
		254	`}`
		255	`}`
		256	`}`
		257	`}`
		258
		259	`if (found)`
		260	`{`
		261	`next = j;`
		262	`// got a valid message, see if any data needs to be discarded`
		263	`event = 0;`
		264
		265	`switch (buf[i+2])`
		266	`{`
		267	`case MESG_RESPONSE_EVENT_ID:`
		268	`if (rfn)`
		269	`{`
		270	`memcpy(rbufp, buf+i+3, dlen);`
		271
		272	`if (!(this->*rfn)(buf[i+3], buf[i+5]))`
		273	`return false;`
		274	`}`
		275	`break;`
		276
		277	`case MESG_BROADCAST_DATA_ID:`
		278	`event = EVENT_RX_BROADCAST;`
		279	`break;`
		280
		281	`case MESG_ACKNOWLEDGED_DATA_ID:`
		282	`event = EVENT_RX_ACKNOWLEDGED;`
		283	`break;`
		284
		285	`case MESG_BURST_DATA_ID:`
		286	`event = EVENT_RX_BURST_PACKET;`
		287	`break;`
		288
		289	`case MESG_EXT_BROADCAST_DATA_ID:`
		290	`event = EVENT_RX_EXT_BROADCAST;`
		291	`break;`
		292
		293	`case MESG_EXT_ACKNOWLEDGED_DATA_ID:`
		294	`event = EVENT_RX_EXT_ACKNOWLEDGED;`
		295	`break;`
		296
		297	`case MESG_EXT_BURST_DATA_ID:`
		298	`event = EVENT_RX_EXT_BURST_PACKET;`
		299	`break;`
		300
		301	`default:`
		302	`if (rfn)`
		303	`{`
		304	`// should be this according to the docs, but doesn't fit`
		305	`if (dlen > MESG_DATA_SIZE)`
		306	`{`
		307	`KMessageBox::error(0, i18n("rresponse buffer too small!"));`
		308	`return false;`
		309	`}`
		310
		311	`memcpy(rbufp, buf+i+3, dlen);`
		312
		313	`if (!(this->*rfn)(buf[i+3], buf[i+2]))`
		314	`return false;`
		315	`}`
		316	`}`
		317
		318	`if (event)`
		319	`{`
		320	`if (cfn)`
		321	`{`
		322	`if (dlen > MESG_DATA_SIZE)`
		323	`{`
		324	`KMessageBox::error(0, i18n("cresponse buffer too small!"));`
		325	`return false;`
		326	`}`
		327
		328	`memcpy(cbufp, buf+i+4, dlen);`
		329
		330	`if (!(this->*cfn)(buf[i+3], event))`
		331	`return false;`
		332	`}`
		333	`}`
		334
		335	`srch = next;`
		336	`}`
		337	`else`
		338	`break;`
		339	`}`
		340
		341	`if (next < bufc)`
		342	`{`
		343	`memmove(buf, buf+next, bufc-next);`
		344	`bufc -= next;`
		345	`}`
		346	`else`
		347	`bufc = 0;`
		348	`}`
		349
		350	`bool ant::ANT_ResetSystem()`
		351	`{`
		352	`uchar filler = 0;`
		353
		354	`return msg_send(MESG_SYSTEM_RESET_ID, &filler, 1);`
		355	`}`
		356
		357	`bool ant::ANT_Cmd55(uchar chan)`
		358	`{`
		359	`return msg_send(0x55, &chan, 1);`
		360	`}`
		361
		362	`bool ant::ANT_OpenRxScanMode (uchar chan)`
		363	`{`
		364	`return msg_send(MESG_OPEN_RX_SCAN_ID, &chan, 1);`
		365	`}`
		366
		367	`bool ant::ANT_Init ()`
		368	`{`
		369	`//char dev[40];`
		370	`struct termios tp;`
		371
		372	`if (!qFile)`
		373	`return false;`
		374
		375	`if (tcgetattr(device.handle(), &tp) < 0)`
		376	`{`
		377	`KMessageBox::error(0, i18n("Error getting terminal attributes!"));`
		378	`return false;`
		379	`}`
		380
		381	`tp.c_iflag &=`
		382	`~(IGNBRK\|BRKINT\|PARMRK\|ISTRIP\|INLCR\|IGNCR\|ICRNL\|IXON\|IXOFF\|IXANY\|INPCK\|IUCLC);`
		383	`tp.c_oflag &= ~OPOST;`
		384	`tp.c_lflag &= ~(ECHO\|ECHONL\|ICANON\|ISIG\|IEXTEN\|ECHOE);`
		385	`tp.c_cflag &= ~(CSIZE\|PARENB);`
		386	`tp.c_cflag \|= CS8 \| CLOCAL \| CREAD \| CRTSCTS;`
		387
		388	`if (cfsetispeed(&tp, B115200) < 0)`
		389	`{`
		390	`KMessageBox::error(0, i18n("Error setting input speed of line terminal!"));`
		391	`return false;`
		392	`}`
		393
		394	`if (cfsetospeed(&tp, B115200) < 0)`
		395	`{`
		396	`KMessageBox::error(0, i18n("Error setting output speed of line terminal!"));`
		397	`return false;`
		398	`}`
		399
		400	`tp.c_cc[VMIN] = 1;`
		401	`tp.c_cc[VTIME] = 0;`
		402
		403	`if (tcsetattr(device.handle (), TCSANOW, &tp) < 0)`
		404	`{`
		405	`KMessageBox::error(0, i18n("Error setting terminal attributes!"));`
		406	`return false;`
		407	`}`
		408
		409	`// return commfn ();`
		410	`return true;`
		411	`}`
		412
		413	`bool ant::ANT_RequestMessage (uchar chan, uchar mesg)`
		414	`{`
		415	`return msg_send(MESG_REQUEST_ID, chan, mesg);`
		416	`}`
		417
		418	`bool ant::ANT_SetNetworkKeya (uchar net, uchar *key)`
		419	`{`
		420	`uchar buf[9];`
		421	`int i;`
		422
		423	`if (strlen((char *)key) != 16)`
		424	`{`
		425	`fprintf(stderr, "Bad key length %s\n", key);`
		426	`return 0;`
		427	`}`
		428
		429	`buf[0] = net;`
		430
		431	`for (i = 0; i < 8; i++)`
		432	`buf[1+i] = hexval(key[i2])16+hexval(key[i*2+1]);`
		433
		434	`return msg_send(MESG_NETWORK_KEY_ID, buf, 9);`
		435	`}`
		436
		437	`bool ant::ANT_SetNetworkKey (uchar net, uchar *key)`
		438	`{`
		439	`uchar buf[9];`
		440	`//int i;`
		441
		442	`buf[0] = net;`
		443	`memcpy(buf+1, key, 8);`
		444	`return msg_send(MESG_NETWORK_KEY_ID, buf, 9);`
		445	`}`
		446
		447	`bool ant::ANT_AssignChannel (uchar chan, uchar chtype, uchar net)`
		448	`{`
		449	`return msg_send(MESG_ASSIGN_CHANNEL_ID, chan, chtype, net);`
		450	`}`
		451
		452	`bool ant::ANT_UnAssignChannel (uchar chan)`
		453	`{`
		454	`return msg_send(MESG_UNASSIGN_CHANNEL_ID, &chan, 1);`
		455	`}`
		456
		457	`bool ant::ANT_SetChannelId (uchar chan, ushort dev, uchar devtype, uchar manid)`
		458	`{`
		459	`uchar buf[5];`
		460
		461	`buf[0] = chan;`
		462	`buf[1] = dev%256;`
		463	`buf[2] = dev/256;`
		464	`buf[3] = devtype;`
		465	`buf[4] = manid;`
		466	`return msg_send(MESG_CHANNEL_ID_ID, buf, 5);`
		467	`}`
		468
		469	`bool ant::ANT_SetChannelRFFreq (uchar chan, uchar freq)`
		470	`{`
		471	`return msg_send(MESG_CHANNEL_RADIO_FREQ_ID, chan, freq);`
		472	`}`
		473
		474	`bool ant::ANT_SetChannelPeriod (uchar chan, ushort period)`
		475	`{`
		476	`uchar buf[3];`
		477
		478	`buf[0] = chan;`
		479	`buf[1] = period%256;`
		480	`buf[2] = period/256;`
		481	`return msg_send(MESG_CHANNEL_MESG_PERIOD_ID, buf, 3);`
		482	`}`
		483
		484	`bool ant::ANT_SetChannelSearchTimeout (uchar chan, uchar timeout)`
		485	`{`
		486	`return msg_send(MESG_CHANNEL_SEARCH_TIMEOUT_ID, chan, timeout);`
		487	`}`
		488
		489	`bool ant::ANT_SetSearchWaveform (uchar chan, ushort waveform)`
		490	`{`
		491	`uchar buf[3];`
		492
		493	`buf[0] = chan;`
		494	`buf[1] = waveform % 256;`
		495	`buf[2] = waveform / 256;`
		496	`return msg_send(MESG_SEARCH_WAVEFORM_ID, buf, 3);`
		497	`}`
		498
		499	`bool ant::ANT_SendAcknowledgedDataA (uchar chan, uchar *data) // ascii version`
		500	`{`
		501	`uchar buf[9];`
		502	`int i;`
		503
		504	`if (strlen((char *)data) != 16)`
		505	`{`
		506	`fprintf(stderr, "Bad data length %s\n", data);`
		507	`return false;`
		508	`}`
		509
		510	`buf[0] = chan;`
		511
		512	`for (i = 0; i < 8; i++)`
		513	`buf[1+i] = hexval (data[i2]) 16 + hexval(data[i*2+1]);`
		514
		515	`return msg_send(MESG_ACKNOWLEDGED_DATA_ID, buf, 9);`
		516	`}`
		517
		518	`bool ant::ANT_SendAcknowledgedData (uchar chan, uchar *data)`
		519	`{`
		520	`uchar buf[9];`
		521	`//int i;`
		522
		523	`buf[0] = chan;`
		524	`memcpy(buf+1, data, 8);`
		525	`return msg_send(MESG_ACKNOWLEDGED_DATA_ID, buf, 9);`
		526	`}`
		527
		528	`unsigned short ant::ANT_SendBurstTransferA(uchar chan, uchar *data, unsigned short numpkts)`
		529	`{`
		530	`uchar buf[9];`
		531	`int i;`
		532	`int j;`
		533	`int seq = 0;`
		534
		535	`fprintf(stderr, "numpkts %d data %s\n", numpkts, data);`
		536
		537	`if (strlen((char )data) != (16 numpkts))`
		538	`{`
		539	`fprintf(stderr, "Bad data length %s numpkts %d\n", data, numpkts);`
		540	`return 0;`
		541	`}`
		542
		543	`for (j = 0; j < numpkts; j++)`
		544	`{`
		545	`buf[0] = chan \| (seq << 5) \| ((j == (numpkts - 1)) ? 0x80 : 0);`
		546
		547	`for (i = 0; i < 8; i++)`
		548	`buf[1+i] = hexval (data[j16+i2]) * 16 + hexval (data[j16+i2+1]);`
		549
		550	`usleep(20 * 1000);`
		551	`msg_send(MESG_BURST_DATA_ID, buf, 9);`
		552	`seq++;`
		553
		554	`if (seq > 3)`
		555	`seq = 1;`
		556	`}`
		557
		558	`return numpkts;`
		559	`}`
		560
		561	`unsigned short ant::ANT_SendBurstTransfer (uchar chan, uchar *data, unsigned short numpkts)`
		562	`{`
		563	`uchar buf[9];`
		564	`//int i;`
		565	`int j;`
		566	`int seq = 0;`
		567
		568	`for (j = 0; j < numpkts; j++)`
		569	`{`
		570	`buf[0] = chan \| (seq << 5) \| ((j == (numpkts - 1)) ? 0x80 : 0);`
		571	`memcpy (buf+1, data+j*8, 8);`
		572	`usleep (20*1000);`
		573	`msg_send (MESG_BURST_DATA_ID, buf, 9);`
		574	`seq++;`
		575
		576	`if (seq > 3)`
		577	`seq = 1;`
		578	`}`
		579
		580	`return numpkts;`
		581	`}`
		582
		583	`bool ant::ANT_OpenChannel (uchar chan)`
		584	`{`
		585	`return msg_send(MESG_OPEN_CHANNEL_ID, &chan, 1);`
		586	`}`
		587
		588	`bool ant::ANT_CloseChannel (uchar chan)`
		589	`{`
		590	`return msg_send(MESG_CLOSE_CHANNEL_ID, &chan, 1);`
		591	`}`
		592
		593	`void ant::ANT_AssignResponseFunction (RESPONSE_FUNC rf, uchar* rbuf)`
		594	`{`
		595	`rfn = rf;`
		596	`rbufp = rbuf;`
		597	`}`
		598
		599	`void ant::ANT_AssignChannelEventFunction (uchar, CHANNEL_EVENT_FUNC rf, uchar* rbuf)`
		600	`{`
		601	`cfn = rf;`
		602	`cbufp = rbuf;`
		603	`}`
		604
		605	`int ant::ANT_fd()`
		606	`{`
		607	`return device.handle();`
		608	`}`
		609
		610	`/*`
		611	`* Following functions control the device and uses the class "ant".`
		612	`* This is class "gant".`
		613	`*/`
		614
		615	`gant::gant (const QString &af)`
		616	`{`
		617	`mydev = 0;`
		618	`peerdev = 0;`
		619	`myid = 0;`
		620	`state = 0;`
		621	`authfd = false;`
		622
		623	`faf.setName (af);`
		624
		625	`if (!faf.open (IO_Raw \| IO_ReadWrite))`
		626	`{`
		627	`KMessageBox::error (0, i18n("Error opening device %1.").arg(af),`
		628	`faf.errorString());`
		629	`}`
		630
		631	`authfile = af;`
		632	`authfd = true;`
		633	`}`
		634
		635	`gant::~gant ()`
		636	`{`
		637	`if (authfd)`
		638	`faf.close();`
		639	`}`
		640
		641	`uint gant::randno ()`
		642	`{`
		643	`return (uint)random ();`
		644	`}`
		645
		646	`bool gant::chevent (uchar chan, uchar event)`
		647	`{`
		648	`uchar seq;`
		649	`uchar last;`
		650	`uchar status = cbuf[1];`
		651	`uchar phase = cbuf[2];`
		652	`struct ack_msg ack;`
		653	`struct auth_msg auth;`
		654	`struct pair_msg pair;`
		655	`uint id;`
		656	`//int i;`
		657
		658	`//fprintf(stderr, "state %d\n", state);`
		659	`/* if (dbg && event != EVENT_RX_BURST_PACKET) {`
		660	`fprintf(stderr, "chan %d event %02x channel open: ", chan, event);`
		661	`for (i = 0; i < 8; i++)`
		662	`fprintf(stderr, "%02x", cbuf[i]);`
		663	`fprintf(stderr, "\n");`
		664	`}`
		665	`*/`
		666	`// transition between garmin phases handled via state machine`
		667	`switch (event)`
		668	`{`
		669	`case EVENT_RX_BROADCAST:`
		670	`memcpy((void *)&id, cbuf+4, 4);`
		671
		672	`if (state == 1 && (phase & 7) == 0)`
		673	`{`
		674	`// received broadcast from watch in phase 0`
		675	`state++;`
		676
		677	`if ((status & 8) == 8)`
		678	`{`
		679	`// pairing`
		680	`myid = randno();`
		681	`fprintf(stderr, "pairing, generated id %08x\n", myid);`
		682	`}`
		683	`else`
		684	`{`
		685	`// garmin moved to phase 1 in response to sendack1`
		686	`int nr;`
		687	`// printf("reading auth data from %s\n", authfile);`
		688	`authfd = open(authfile, O_RDONLY);`
		689
		690	`if (!authfd \|\| faf.size () < 32)`
		691	`{`
		692	`KMessageBox::error(0, i18n("There is no authfile available or it has wrong size!"));`
		693	`return false;`
		694	`}`
		695
		696	`nr = read(faf.handle(), authdata, 32);`
		697
		698	`if (nr != 32)`
		699	`{`
		700	`KMessageBox::error(0, i18n ("Error reading auth file %1!").arg(authfile));`
		701	`return false;`
		702	`}`
		703
		704	`memcpy((void *)&myauth1, authdata+16, 4);`
		705	`memcpy((void *)&myauth2, authdata+20, 4);`
		706	`memcpy((void *)&mydev, authdata+12, 4);`
		707	`memcpy((void *)&myid, authdata+4, 4);`
		708	`}`
		709
		710	`if (status & 0x20)`
		711	`fprintf(stderr, "watch contains NEW data\n");`
		712
		713	`ANT_RequestMessage(chan, MESG_CHANNEL_ID_ID); /* request sender id */`
		714	`}`
		715	`else if (state == 3 && phase == 1)`
		716	`{`
		717	`state++;`
		718	`ack.code = 0x44;`
		719	`ack.atype = 4;`
		720	`ack.c1 = 0x01;`
		721	`ack.c2 = 0x00;`
		722	`ack.id = myid;`
		723	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // tell garmin to send id data`
		724	`}`
		725	`else if (state == 5 && phase == 1)`
		726	`{`
		727	`if ((status & 0x0f) == 4)`
		728	`{`
		729	`// received id/version or whatever`
		730	`state++;`
		731	`assert(sizeof auth == AUTHSIZE);`
		732	`auth.code = 0x44;`
		733	`auth.atype = 4;`
		734	`auth.phase = 3;`
		735	`auth.u1 = 8;`
		736	`auth.id = myid;`
		737	`auth.auth1 = myauth1;`
		738	`auth.auth2 = myauth2;`
		739	`auth.fill1 = auth.fill2 = 0;`
		740	`ANT_SendBurstTransfer(chan, (uchar *)&auth, (sizeof auth)/8); // send our auth data`
		741	`}`
		742	`else if ((status & 0x0f) == 0x0c)`
		743	`{`
		744	`// pairing`
		745	`assert(sizeof pair == PAIRSIZE);`
		746	`state = 10;`
		747	`pair.code = 0x44;`
		748	`pair.atype = 4;`
		749	`pair.phase = 2;`
		750	`pair.u1 = 7;`
		751	`pair.id = myid;`
		752	`bzero(pair.devname, sizeof pair.devname);`
		753
		754	`if (peerdev <= 99999999) // only allow 8 digits`
		755	`sprintf(pair.devname, "%d", peerdev);`
		756	`else`
		757	`fprintf(stderr, "pair dev name too large %08x \"%d\"\n", peerdev, peerdev);`
		758
		759	`// printf("sending pair data for dev %s\n", pair.devname);`
		760	`ANT_SendBurstTransfer(chan, (uchar *)&pair, (sizeof pair)/8) ; // send pair data`
		761	`}`
		762	`else`
		763	`{`
		764	`fprintf(stderr, "error in gant.cpp on line %d\n", __LINE__);`
		765	`return false;`
		766	`}`
		767	`}`
		768	`else if (state == 6 && (status & 0x0f) == 4 && phase == 2)`
		769	`{`
		770	`// if our auth is ok, garmin has gone to phase 2`
		771	`state++;`
		772	`ack.code = 0x44;`
		773	`ack.atype = 6;`
		774	`ack.c1 = 0x01;`
		775	`ack.c2 = 0x00;`
		776	`ack.id = 0;`
		777	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // tell garmin to start upload`
		778	`}`
		779	`else if (state == 8 && phase == 2)`
		780	`{`
		781	`// upload finished. received broadcast that still in phase 2`
		782	`state++;`
		783	`ack.code = 0x44;`
		784	`ack.atype = 3;`
		785	`ack.c1 = 0x00;`
		786	`ack.c2 = 0x00;`
		787	`ack.id = 0;`
		788	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // tell garmin we're finished`
		789	`}`
		790	`else if (state == 9 && phase == 0)`
		791	`{`
		792	`// garmin replies it's back in phase 0`
		793	`fprintf(stderr, "finished\n");`
		794	`return true;`
		795	`}`
		796	`else if (state == 1 && phase != 0)`
		797	`{`
		798	`// don't know what phase we're in. let's try and reset`
		799	`fprintf(stderr, "resetting\n");`
		800	`ack.code = 0x44;`
		801	`ack.atype = 3;`
		802	`ack.c1 = 0x00;`
		803	`ack.c2 = 0x00;`
		804	`ack.id = 0;`
		805	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // tell garmin we're finished`
		806	`sleep(1);`
		807	`return false;`
		808	`}`
		809	`else if (state == 20 && phase == 2)`
		810	`{`
		811	`// upload finished. tell garmin to delete logs`
		812	`state++;`
		813	`ack.code = 0x44;`
		814	`ack.atype = 0x0b;`
		815	`ack.c1 = 0x01;`
		816	`ack.c2 = 0x00;`
		817	`ack.id = 0;`
		818	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // delete logs`
		819	`}`
		820	`else if (state == 21 && phase == 2)`
		821	`{`
		822	`state = 8;`
		823	`}`
		824	`break;`
		825
		826	`case EVENT_RX_BURST_PACKET:`
		827	`seq = (chan & 0x60) >> 5;`
		828	`last = (chan & 0x80) >> 7;`
		829	`chan &= 0x1f;`
		830
		831	`if (state == 4)`
		832	`{`
		833	`static int once = 0;`
		834	`// garmin sending authentication/identification data`
		835
		836	`if (!once)`
		837	`once = 1;`
		838
		839	`memcpy(clientid[seq], cbuf, 8);`
		840
		841	`if (last)`
		842	`{`
		843	`state++;`
		844
		845	`memcpy(&peerdev, clientid[1]+4, 4);`
		846
		847	`if (mydev != 0 && peerdev != mydev)`
		848	`{`
		849	`fprintf(stderr, "Don't know this device %08x != %08x\n", peerdev, mydev);`
		850	`return false;`
		851	`}`
		852	`}`
		853	`}`
		854	`else if (state == 7)`
		855	`{`
		856	`static int once = 0;`
		857	`int nw;`
		858
		859	`// garmin uploading in response to sendack3`
		860	`// in this state we're receiving the workout data`
		861	`if (!once)`
		862	`{`
		863	`fprintf(stderr, "receiving\n");`
		864	`once = 1;`
		865	`}`
		866
		867	`nw = write(faf.handle(), cbuf, 8);`
		868
		869	`if (nw != 8)`
		870	`{`
		871	`KMessageBox::error(0, i18n ("Write to auth file %1 failed!").arg(authfile));`
		872	`return false;`
		873	`}`
		874
		875	`if (last)`
		876	`{`
		877	`state++; // just to exit`
		878	`//state = 20; // to delete logs`
		879	`}`
		880	`}`
		881	`else if (state == 10)`
		882	`{`
		883	`// receiving auth data`
		884	`static int once = 0;`
		885	`int nw;`
		886
		887	`if (!once)`
		888	`{`
		889	`// printf("storing auth data in %s\n", authfile);`
		890	`once = 1;`
		891	`// authfd = open(authfile, O_WRONLY\|O_CREAT, 0644);`
		892
		893	`/* if (authfd < 0)`
		894	`{`
		895	`perror(authfile);`
		896	`exit(1);`
		897	`} */`
		898	`}`
		899
		900	`nw = write(faf.handle(), cbuf, 8);`
		901
		902	`if (nw != 8)`
		903	`{`
		904	`KMessageBox::error(0, i18n ("Write to auth file %1 failed!").arg(authfile));`
		905	`return false;`
		906	`}`
		907
		908	`if (last)`
		909	`{`
		910	`//state = 6; // to download during pairing`
		911	`state = 8; // to finish at end of pairing`
		912	`}`
		913	`}`
		914	`else if (state == 6)`
		915	`{`
		916	`// response to authentication`
		917	`static int once = 0;`
		918	`// int i;`
		919
		920	`if (!once)`
		921	`{`
		922	`once = 1;`
		923	`}`
		924
		925	`if (last)`
		926	`{`
		927	`if (cbuf[2] == 2)`
		928	`{`
		929	`fprintf(stderr, "authentication failed\n");`
		930	`return false;`
		931	`}`
		932	`}`
		933	`}`
		934	`else`
		935	`{`
		936	`int i;`
		937
		938	`fprintf(stderr, "data in state %d: ", state);`
		939
		940	`for (i = 0; i < 8; i++)`
		941	`fprintf(stderr, "%02x", cbuf[i]);`
		942
		943	`fprintf(stderr, "\n");`
		944	`}`
		945	`break;`
		946	`}`
		947
		948	`return 1;`
		949	`}`
		950
		951	`bool gant::revent (uchar chan, uchar event)`
		952	`{`
		953	`unsigned short devid;`
		954	`struct ack_msg ack;`
		955	`int i;`
		956
		957	`switch (event)`
		958	`{`
		959	`case EVENT_TRANSFER_TX_COMPLETED:`
		960	`// ignore`
		961	`// printf("Transfer complete %02x\n", ebuf[1]);`
		962	`break;`
		963
		964	`case INVALID_MESSAGE:`
		965	`fprintf(stderr, "Invalid message %02x\n", ebuf[1]);`
		966	`break;`
		967
		968	`case RESPONSE_NO_ERROR:`
		969	`switch (ebuf[1])`
		970	`{`
		971	`case MESG_ASSIGN_CHANNEL_ID:`
		972	`ANT_AssignChannelEventFunction((uchar)chan, (CHANNEL_EVENT_FUNC)&gant::chevent, (uchar *)&cbuf);`
		973	`break;`
		974
		975	`case MESG_OPEN_CHANNEL_ID:`
		976	`state++;`
		977	`fprintf(stderr, "channel open, waiting for broadcast\n", state);`
		978	`break;`
		979
		980	`default:`
		981	`// ignore`
		982	`// printf("Message %02x NO_ERROR\n", ebuf[1]);`
		983	`break;`
		984	`}`
		985	`break;`
		986
		987	`case MESG_CHANNEL_ID_ID:`
		988	`devid = ebuf[1]+ebuf[2]*256;`
		989
		990	`if (mydev == 0 \|\| devid == mydev%65536)`
		991	`{`
		992	`state++;`
		993	`ack.code = 0x44; ack.atype = 2; ack.c1 = 0x32; ack.c2 = 0x04;`
		994	`ack.id = myid;`
		995	`ANT_SendAcknowledgedData(chan, (uchar *)&ack); // tell watch go to phase 1`
		996	`}`
		997	`else`
		998	`{`
		999	`fprintf(stderr, "Ignoring unknown device %08x, mydev %08x\n", devid, mydev);`
		1000	`}`
		1001	`break;`
		1002
		1003	`case MESG_NETWORK_KEY_ID:`
		1004	`case MESG_SEARCH_WAVEFORM_ID:`
		1005	`case MESG_OPEN_CHANNEL_ID:`
		1006	`fprintf(stderr, "response event %02x code %02x\n", event, ebuf[2]);`
		1007
		1008	`for (i = 0; i < 8; i++)`
		1009	`fprintf(stderr, "%02x", ebuf[i]);`
		1010
		1011	`fprintf(stderr, "\n");`
		1012	`break;`
		1013
		1014	`case MESG_CAPABILITIES_ID:`
		1015	`break;`
		1016
		1017	`case MESG_CHANNEL_STATUS_ID:`
		1018	`break;`
		1019
		1020	`default:`
		1021	`fprintf(stderr, "Unhandled response event %02x\n", event);`
		1022	`break;`
		1023	`}`
		1024
		1025	`return true;`
		1026	`}`
		1027
		1028	`bool gant::read_device ()`
		1029	`{`
		1030	`//int devnum = 0;`
		1031	`int chan = 0;`
		1032	`int net = 0;`
		1033	`int chtype = 0; // wildcard`
		1034	`int devno = 0; // wildcard`
		1035	`int devtype = 0; // wildcard`
		1036	`int manid = 0; // wildcard`
		1037	`int freq = 0x32; // garmin specific radio frequency`
		1038	`int period = 0x1000; // garmin specific broadcast period`
		1039	`int srchto = 255; // max timeout`
		1040	`int waveform = 0x0053; // aids search somehow`
		1041
		1042	`// Check if we already have a device set.`
		1043	`// if (!qFile)`
		1044	`// return false;`
		1045
		1046	`// TODO: Do this properly...`
		1047	`/* if (ac > 1) {`
		1048	`authfile = av[1];`
		1049	`}`
		1050	`else {`
		1051	`fprintf(stderr, "need auth file\n");`
		1052	`exit(1);`
		1053	`}`
		1054
		1055	`if (ac > 2)`
		1056	`fn = av[2]; // store the output filename for event function`
		1057	`if (ac > 3)`
		1058	`devnum = atoi(av[3]);`
		1059	`if (ac > 4)`
		1060	`myid = atoi(av[4]);`
		1061	`if (ac > 5)`
		1062	`mydev = atoi(av[5]);`
		1063	`*/`
		1064	`if (!ANT_Init())`
		1065	`return false;`
		1066
		1067	`if (!ANT_ResetSystem())`
		1068	`return false;`
		1069
		1070	`ANT_AssignResponseFunction((RESPONSE_FUNC)&gant::revent, (uchar *)&ebuf);`
		1071
		1072	`if (!ANT_RequestMessage(chan, MESG_CHANNEL_STATUS_ID)) //informative`
		1073	`return false;`
		1074
		1075	`if (!ANT_SetNetworkKeya(net, (uchar *)G50_KEY))`
		1076	`return false;`
		1077
		1078	`if (!ANT_AssignChannel(chan, chtype, net))`
		1079	`return false;`
		1080
		1081	`if (!ANT_SetChannelId(chan, devno, devtype, manid))`
		1082	`return false;`
		1083
		1084	`if (!ANT_RequestMessage(chan, MESG_CAPABILITIES_ID)) //informative`
		1085	`return false;`
		1086
		1087	`if (!ANT_SetChannelRFFreq(chan, freq))`
		1088	`return false;`
		1089
		1090	`if (!ANT_SetChannelPeriod(chan, period))`
		1091	`return false;`
		1092
		1093	`if (!ANT_SetChannelSearchTimeout(chan, srchto))`
		1094	`return false;`
		1095
		1096	`if (!ANT_SetSearchWaveform(chan, waveform))`
		1097	`return false;`
		1098
		1099	`if (!ANT_OpenChannel(chan)) // success for this bumps us to state 1`
		1100	`return false;`
		1101
		1102	`if (!ANT_RequestMessage(chan, MESG_CHANNEL_STATUS_ID)) //informative`
		1103	`return false;`
		1104
		1105	`// everything handled in event functions`
		1106	`// for(;;)`
		1107	`// sleep(10);`
		1108
		1109	`return commfn ();`
		1110	`}`

Subversion Repositories public

(root)/sportwatcher/trunk/libgant/gant.cpp – Rev 230