driver_garmin.c 47 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312
  1. /*
  2. * This file contains two drivers for Garmin receivers and some code
  3. * shared by both drivers.
  4. *
  5. * One driver "garmin_usb_binary" handles the Garmin binary packet
  6. * format supported by the USB Garmins tested with the Garmin 18 and
  7. * other models. (There is also "garmin_usb_binary_old".) These are ONLY
  8. * for USB devices reporting as: 091e:0003.
  9. *
  10. * The other driver "garmin_ser_binary" is for Garmin receivers via a
  11. * serial port, whether or not one uses a USB/serial adaptor or a real
  12. * serial port. These receivers provide adequate NMEA support, so it
  13. * often makes sense to just put them into NMEA mode.
  14. *
  15. * On Linux, USB Garmins (091e:0003) need the Linux garmin_gps driver and
  16. * will not function without it. On other operating systems, it is clear
  17. * garmin_usb_binary_old does not work since it requires the Linux
  18. * garmin_gps module.
  19. *
  20. * This code has been tested and at least at one time is known to work on
  21. * big- and little-endian CPUs and 32 and 64 bit cpu modes.
  22. *
  23. *
  24. * Documentation for the Garmin protocols can be found via
  25. * http://www.garmin.com/support/commProtocol.html
  26. * The file IOSDK.zip contains IntfSpec.pdf, which describes the
  27. * protocol in terms of Application, Link, and Physical. This
  28. * identical file is also available at:
  29. * http://www.garmin.com/support/pdf/iop_spec.pdf
  30. * An older version of iop_spec.pdf that describes only Serial Binary
  31. * is available at:
  32. * http://vancouver-webpages.com/pub/peter/iop_spec.pdf
  33. * Information about the GPS 18
  34. * http://www.garmin.com/manuals/425_TechnicalSpecification.pdf
  35. *
  36. * There is one physical link protocol for serial which uses DLE/ETX
  37. * framing. There is another physical protocol for USB which relies
  38. * on the packetization intrinsic to USB bulk pipes.
  39. *
  40. * There are several link protocols; all devices implement L000.
  41. * There are then product-specific protocols; most devices implement
  42. * L001. Link protocols are the same and carried over either Physical
  43. * protocol.
  44. *
  45. * Application protocols are named A000 and then with different
  46. * 3-digit numbers. They are carried over Link protocols.
  47. *
  48. * Thus, much of the higher-level code dealing the data formats is
  49. * shared between USB Binary and Serial Binary.
  50. *
  51. * This code is partly from the Garmin IOSDK and partly from the
  52. * sample code in the Linux garmin_gps driver.
  53. *
  54. * bad code by: Gary E. Miller <gem@rellim.com>
  55. *
  56. * -D 3 = packet trace
  57. * -D 4 = packet details
  58. * -D 5 = more packet details
  59. * -D 6 = very excessive details
  60. *
  61. * limitations:
  62. *
  63. * do not have from garmin:
  64. * pdop
  65. * hdop
  66. * vdop
  67. * magnetic variation
  68. *
  69. * known bugs:
  70. * hangs in the fread loop instead of keeping state and returning.
  71. *
  72. * This file is Copyright 2010 by the GPSD project
  73. * SPDX-License-Identifier: BSD-2-clause
  74. */
  75. #include "gpsd_config.h" /* must be before all includes */
  76. #ifdef GARMIN_ENABLE
  77. #include <errno.h>
  78. #include <math.h>
  79. #include <stdbool.h>
  80. #include <stdio.h>
  81. #include <string.h>
  82. #include <time.h>
  83. #include <unistd.h>
  84. #if defined(HAVE_LIBUSB)
  85. #include <libusb.h>
  86. #endif
  87. #include "gpsd.h"
  88. #include "bits.h"
  89. #include "timespec.h"
  90. #define GPSD_LE16TOH(x) getles16((char *)(&(x)), 0)
  91. #define GPSD_LE32TOH(x) getles32((char *)(&(x)), 0)
  92. #define GPSD_LEF32(x) getlef32((const char *)(&(x)), 0)
  93. #define GPSD_LED64(x) getled64((const char *)(&(x)), 0)
  94. #define USE_RMD 0
  95. /* Used in Serial Physical Layer */
  96. #define ETX 0x03
  97. #define ACK 0x06
  98. #define DLE 0x10
  99. #define NAK 0x15
  100. #define GARMIN_LAYERID_TRANSPORT (uint8_t) 0
  101. #define GARMIN_LAYERID_APPL (uint32_t) 20
  102. // Linux Garmin USB driver layer-id to use for some control mechanisms
  103. #define GARMIN_LAYERID_PRIVATE 0x01106E4B
  104. // packet ids used in private layer
  105. #define PRIV_PKTID_SET_DEBUG 1
  106. #define PRIV_PKTID_SET_MODE 2
  107. #define PRIV_PKTID_INFO_REQ 3
  108. #define PRIV_PKTID_INFO_RESP 4
  109. #define PRIV_PKTID_RESET_REQ 5
  110. #define PRIV_PKTID_SET_DEF_MODE 6
  111. #define MODE_NATIVE 0
  112. #define MODE_GARMIN_SERIAL 1
  113. #define GARMIN_PKTID_TRANSPORT_START_SESSION_REQ 5
  114. #define GARMIN_PKTID_TRANSPORT_START_SESSION_RESP 6
  115. #define GARMIN_PKTID_PROTOCOL_ARRAY 253
  116. #define GARMIN_PKTID_PRODUCT_RQST 254
  117. #define GARMIN_PKTID_PRODUCT_DATA 255
  118. /* 0x29 ')' */
  119. #define GARMIN_PKTID_RMD41_DATA 41
  120. /* 0x33 '3' */
  121. #define GARMIN_PKTID_PVT_DATA 51
  122. /* 0x33 '4' */
  123. #define GARMIN_PKTID_RMD_DATA 52
  124. /* 0x72 'r' */
  125. #define GARMIN_PKTID_SAT_DATA 114
  126. #define GARMIN_PKTID_L001_XFER_CMPLT 12
  127. #define GARMIN_PKTID_L001_COMMAND_DATA 10
  128. #define GARMIN_PKTID_L001_DATE_TIME_DATA 14
  129. #define GARMIN_PKTID_L001_RECORDS 27
  130. #define GARMIN_PKTID_L001_WPT_DATA 35
  131. #define CMND_ABORT 0
  132. #define CMND_START_PVT_DATA 49
  133. #define CMND_STOP_PVT_DATA 50
  134. #define CMND_START_RM_DATA 110
  135. #define MAX_BUFFER_SIZE 4096
  136. #define GARMIN_CHANNELS 12
  137. // something magic about 64, garmin driver will not return more than
  138. // 64 at a time. If you read less than 64 bytes the next read will
  139. // just get the last of the 64 byte buffer.
  140. #define ASYNC_DATA_SIZE 64
  141. #pragma pack(1)
  142. // This is the data format of the satellite data from the garmin USB
  143. typedef struct __attribute__((__packed__))
  144. {
  145. uint8_t svid;
  146. uint16_t snr; // 0 - 0xffff
  147. uint8_t elev;
  148. uint16_t azmth;
  149. uint8_t status; // bit 0, has ephemeris, 1, has diff correction
  150. // bit 2 used in solution
  151. // bit 3??
  152. } cpo_sat_data;
  153. /* Garmin D800_Pvt_Datetype_Type */
  154. /* packet type: GARMIN_PKTID_PVT_DATA 52 */
  155. /* This is the data format of the position data from the garmin USB */
  156. typedef struct __attribute__((__packed__))
  157. {
  158. float alt; /* altitude above WGS 84 (meters) */
  159. float epe; /* estimated position error, 2 sigma (meters) */
  160. float eph; /* epe, but horizontal only (meters) */
  161. float epv; /* epe but vertical only (meters ) */
  162. int16_t fix; /* 0 - failed integrity check
  163. * 1 - invalid or unavailable fix
  164. * 2 - 2D
  165. * 3 - 3D
  166. * 4 - 2D Diff
  167. * 5 - 3D Diff
  168. */
  169. double gps_tow; /* gps time of week (seconds) */
  170. double lat; /* ->latitude (radians) */
  171. double lon; /* ->longitude (radians) */
  172. float lon_vel; /* velocity east (meters/second) */
  173. float lat_vel; /* velocity north (meters/second) */
  174. float alt_vel; /* velocity up (meters/sec) */
  175. // Garmin GPS25 uses pkt_id 0x28 and does not output the
  176. // next 3 items
  177. float msl_hght; /* height of WGS 84 above MSL (meters) */
  178. int16_t leap_sec; /* diff between GPS and UTC (seconds) */
  179. int32_t grmn_days; /* days from UTC December 31st, 1989 to the
  180. * beginning of the current week */
  181. } cpo_pvt_data;
  182. typedef struct __attribute__((__packed__))
  183. {
  184. uint32_t cycles;
  185. double pr; // pseudorange in meters
  186. uint16_t phase;
  187. int8_t slp_dtct;
  188. uint8_t snr_dbhz;
  189. uint8_t svid;
  190. int8_t valid;
  191. } cpo_rcv_sv_data;
  192. /* packet type: GARMIN_PKTID_RMD_DATA 53 */
  193. /* seems identical to the packet id 0x29 from the Garmin GPS 25 */
  194. typedef struct __attribute__((__packed__))
  195. {
  196. double rcvr_tow;
  197. int16_t rcvr_wn;
  198. cpo_rcv_sv_data sv[GARMIN_CHANNELS];
  199. } cpo_rcv_data;
  200. // This is the packet format to/from the Garmin USB
  201. typedef struct __attribute__((__packed__))
  202. {
  203. uint8_t mPacketType;
  204. uint8_t mReserved1;
  205. uint16_t mReserved2;
  206. uint16_t mPacketId;
  207. uint16_t mReserved3;
  208. uint32_t mDataSize;
  209. union
  210. {
  211. //int8_t chars[MAX_BUFFER_SIZE];
  212. // cppcheck-suppress unusedStructMember
  213. uint8_t uchars[MAX_BUFFER_SIZE];
  214. cpo_pvt_data pvt;
  215. cpo_sat_data sats;
  216. } mData;
  217. } Packet_t;
  218. // useful funcs to read/write ints
  219. // floats and doubles are Intel (little-endian) order only...
  220. static inline void set_int16(uint8_t * buf, uint32_t value)
  221. {
  222. buf[0] = (uint8_t) (0x0FF & value);
  223. buf[1] = (uint8_t) (0x0FF & (value >> 8));
  224. }
  225. static inline void set_int32(uint8_t * buf, uint32_t value)
  226. {
  227. buf[0] = (uint8_t) (0x0FF & value);
  228. buf[1] = (uint8_t) (0x0FF & (value >> 8));
  229. buf[2] = (uint8_t) (0x0FF & (value >> 16));
  230. buf[3] = (uint8_t) (0x0FF & (value >> 24));
  231. }
  232. static inline uint16_t get_uint16(const uint8_t * buf)
  233. {
  234. return (uint16_t) (0xFF & buf[0])
  235. | ((uint16_t) (0xFF & buf[1]) << 8);
  236. }
  237. #if defined(HAVE_LIBUSB) && defined(__linux__)
  238. static inline uint32_t get_int32(const uint8_t * buf)
  239. {
  240. return (uint32_t) (0xFF & buf[0])
  241. | ((uint32_t) (0xFF & buf[1]) << 8)
  242. | ((uint32_t) (0xFF & buf[2]) << 16)
  243. | ((uint32_t) (0xFF & buf[3]) << 24);
  244. }
  245. #endif /* HAVE_LIBUSB */
  246. // convert radians to degrees
  247. static inline double radtodeg(double rad)
  248. {
  249. return (double)(rad * RAD_2_DEG);
  250. }
  251. static gps_mask_t PrintSERPacket(struct gps_device_t *session,
  252. unsigned char pkt_id, int pkt_len,
  253. unsigned char *buf);
  254. #if defined(HAVE_LIBUSB) && defined(__linux__)
  255. static gps_mask_t PrintUSBPacket(struct gps_device_t *session,
  256. Packet_t * pkt);
  257. #endif /* HAVE_LIBUSB */
  258. gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id,
  259. int pkt_len, unsigned char *buf)
  260. {
  261. gps_mask_t mask = ONLINE_SET;
  262. int i = 0, j = 0;
  263. uint16_t prod_id = 0;
  264. uint16_t ver = 0;
  265. int maj_ver;
  266. int min_ver;
  267. time_t time_l = 0;
  268. char msg_buf[512] = "";
  269. char *msg = NULL;
  270. cpo_sat_data *sats = NULL;
  271. cpo_pvt_data *pvt = NULL;
  272. cpo_rcv_data *rmd = NULL;
  273. double gps_tow = 0;
  274. GPSD_LOG(LOG_DATA, &session->context->errout,
  275. "Garmin: PrintSERPacket(, %#02x, %#02x, )\n", pkt_id, pkt_len);
  276. session->cycle_end_reliable = true;
  277. switch (pkt_id) {
  278. case ACK:
  279. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: ACK\n");
  280. break;
  281. case NAK:
  282. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: NAK\n");
  283. break;
  284. case GARMIN_PKTID_L001_COMMAND_DATA:
  285. prod_id = get_uint16((uint8_t *) buf);
  286. switch (prod_id) {
  287. case CMND_ABORT:
  288. msg = "Abort current xfer";
  289. break;
  290. case CMND_START_PVT_DATA:
  291. msg = "Start Xmit PVT data";
  292. break;
  293. case CMND_STOP_PVT_DATA:
  294. msg = "Stop Xmit PVT data";
  295. break;
  296. case CMND_START_RM_DATA:
  297. msg = "Start RMD data";
  298. break;
  299. default:
  300. (void)snprintf(msg_buf, sizeof(msg_buf), "Unknown: %u",
  301. (unsigned int)prod_id);
  302. msg = msg_buf;
  303. break;
  304. }
  305. GPSD_LOG(LOG_PROG, &session->context->errout,
  306. "Garmin: Appl, Command Data: %s\n", msg);
  307. break;
  308. case GARMIN_PKTID_PRODUCT_RQST:
  309. GPSD_LOG(LOG_PROG, &session->context->errout,
  310. "Garmin: Appl, Product Data req\n");
  311. break;
  312. case GARMIN_PKTID_PRODUCT_DATA:
  313. prod_id = get_uint16((uint8_t *) buf);
  314. ver = get_uint16((uint8_t *) & buf[2]);
  315. maj_ver = (int)(ver / 100);
  316. min_ver = (int)(ver - (maj_ver * 100));
  317. GPSD_LOG(LOG_PROG, &session->context->errout,
  318. "Garmin: Appl, Product Data, sz: %d\n",
  319. pkt_len);
  320. (void)snprintf(session->subtype, sizeof(session->subtype),
  321. "%d: %d.%02d", (int)prod_id, maj_ver, min_ver);
  322. GPSD_LOG(LOG_INF, &session->context->errout,
  323. "Garmin: Product ID: %d, SoftVer: %d.%02d\n",
  324. prod_id, maj_ver, min_ver);
  325. GPSD_LOG(LOG_INF, &session->context->errout,
  326. "Garmin: Product Desc: %s\n", &buf[4]);
  327. mask |= DEVICEID_SET;
  328. GPSD_LOG(LOG_DATA, &session->context->errout,
  329. "Garmin: PRODUCT_DATA: subtype=%s\n",
  330. session->subtype);
  331. break;
  332. case GARMIN_PKTID_PVT_DATA:
  333. GPSD_LOG(LOG_PROG, &session->context->errout,
  334. "Garmin: PVT Data (51) Sz: %d\n", pkt_len);
  335. pvt = (cpo_pvt_data *) buf;
  336. session->context->leap_seconds = (int)GPSD_LE16TOH(pvt->leap_sec);
  337. session->context->valid = LEAP_SECOND_VALID;
  338. // 631065600, unix seconds for 31 Dec 1989 Zulu
  339. time_l = (time_t) (631065600 + (GPSD_LE32TOH(pvt->grmn_days) * 86400));
  340. // TODO, convert grmn_days to context->gps_week
  341. time_l -= session->context->leap_seconds;
  342. // gps_tow is always like x.999 or x.998 just round it to nearest sec
  343. // FIXME! this will break 5Hz garmins...
  344. gps_tow = GPSD_LED64(pvt->gps_tow);
  345. time_l += (time_t)round(gps_tow);
  346. /* sanity check unix time against leap second.
  347. * Leap second 18 at 1 Jan 2017: 1483228800 */
  348. if (17 < session->context->leap_seconds &&
  349. 1483228800L > time_l) {
  350. time_l += 619315200; // fast forward 1024 weeks
  351. }
  352. DTOTS(&session->context->gps_tow, gps_tow);
  353. session->newdata.time.tv_sec = time_l;
  354. session->newdata.time.tv_nsec = 0;
  355. // (long long) for 32-bit systems
  356. GPSD_LOG(LOG_PROG, &session->context->errout,
  357. "Garmin: time_l: %lld\n", (long long)time_l);
  358. session->newdata.latitude = radtodeg(GPSD_LED64(pvt->lat));
  359. session->newdata.longitude = radtodeg(GPSD_LED64(pvt->lon));
  360. // altitude is WGS84
  361. session->newdata.altHAE = GPSD_LEF32(pvt->alt);
  362. // geoid separation from WGS 84
  363. // gpsd sign is opposite of garmin sign
  364. session->newdata.geoid_sep = -GPSD_LEF32(pvt->msl_hght);
  365. /* Estimated position error in meters. Confidence (sigma) not
  366. * specified by Garmin.
  367. * We follow the advice at <http://gpsinformation.net/main/errors.htm>.
  368. * Since GPS data is not gaussian, this is marginal advice...
  369. * If this assumption changes here, it should also change in
  370. * nmea_parse.c where we analyze PGRME.
  371. */
  372. session->newdata.sep = GPSD_LEF32(pvt->epe) *
  373. (GPSD_CONFIDENCE / CEP50_SIGMA);
  374. /* eph, horizaontal error, 2 sigma */
  375. session->newdata.eph = GPSD_LEF32(pvt->eph) *
  376. (GPSD_CONFIDENCE / CEP50_SIGMA);
  377. /* eph, horizaontal error, 2 sigma */
  378. session->newdata.epv = GPSD_LEF32(pvt->epv) *
  379. (GPSD_CONFIDENCE / CEP50_SIGMA);
  380. /* meters/sec */
  381. session->newdata.NED.velN = GPSD_LEF32(pvt->lat_vel);
  382. session->newdata.NED.velE = GPSD_LEF32(pvt->lon_vel);
  383. session->newdata.NED.velD = -GPSD_LEF32(pvt->alt_vel);
  384. switch (GPSD_LE16TOH(pvt->fix)) {
  385. case 0:
  386. case 1:
  387. default:
  388. // no fix
  389. session->newdata.status = STATUS_NO_FIX;
  390. session->newdata.mode = MODE_NO_FIX;
  391. break;
  392. case 2:
  393. // 2D fix
  394. session->newdata.status = STATUS_FIX;
  395. session->newdata.mode = MODE_2D;
  396. break;
  397. case 3:
  398. // 3D fix
  399. session->newdata.status = STATUS_FIX;
  400. session->newdata.mode = MODE_3D;
  401. break;
  402. case 4:
  403. // 2D Differential fix
  404. session->newdata.status = STATUS_DGPS_FIX;
  405. session->newdata.mode = MODE_2D;
  406. break;
  407. case 5:
  408. // 3D differential fix
  409. session->newdata.status = STATUS_DGPS_FIX;
  410. session->newdata.mode = MODE_3D;
  411. break;
  412. }
  413. GPSD_LOG(LOG_PROG, &session->context->errout,
  414. "Garmin: Appl, mode %d, status %d\n",
  415. session->newdata.mode, session->newdata.status);
  416. /* save some expensive calculations if not needed */
  417. if (session->context->errout.debug >= LOG_INF) {
  418. GPSD_LOG(LOG_INF, &session->context->errout,
  419. "Garmin: UTC Time: %lld\n",
  420. (long long)session->newdata.time.tv_sec);
  421. GPSD_LOG(LOG_INF, &session->context->errout,
  422. "Garmin: Geoid Separation (MSL-WGS84): from garmin %lf, "
  423. "calculated %lf\n",
  424. session->newdata.geoid_sep,
  425. wgs84_separation(session->newdata.latitude,
  426. session->newdata.longitude));
  427. GPSD_LOG(LOG_INF, &session->context->errout,
  428. "Garmin: Alt: %.3f, sep: %.3f, eph: %.3f, Epv: %.3f, "
  429. "Fix: %d, Gps_tow: %f, Lat: %.3f, Lon: %.3f, "
  430. "velN: %.3f, velE: %.3f, velD: %.3f, geoidsep: %.3f, "
  431. "Leap: %d, GarminDays: %d\n",
  432. session->newdata.altHAE,
  433. session->newdata.sep,
  434. session->newdata.eph,
  435. session->newdata.epv,
  436. GPSD_LE16TOH(pvt->fix),
  437. gps_tow,
  438. session->newdata.latitude,
  439. session->newdata.longitude,
  440. session->newdata.NED.velN,
  441. session->newdata.NED.velE,
  442. session->newdata.NED.velD,
  443. session->newdata.geoid_sep,
  444. session->context->leap_seconds,
  445. GPSD_LE32TOH(pvt->grmn_days));
  446. }
  447. if (session->newdata.mode > MODE_NO_FIX) {
  448. /* data only valid with a fix */
  449. mask |=
  450. TIME_SET | LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET |
  451. HERR_SET | PERR_IS | CLEAR_IS | REPORT_IS | VNED_SET;
  452. /*
  453. * Garmin documentation says we should wait until four good fixes
  454. * have been seen before trying to use the device for precision
  455. * time service.
  456. */
  457. if (session->fixcnt > 3)
  458. mask |= NTPTIME_IS;
  459. }
  460. GPSD_LOG(LOG_DATA, &session->context->errout,
  461. "Garmin: PVT_DATA: time=%lld, lat=%.2f lon=%.2f "
  462. "eph=%.2f sep=%.2f epv=%.2f mode=%d status=%d\n",
  463. (long long)session->newdata.time.tv_sec,
  464. session->newdata.latitude,
  465. session->newdata.longitude,
  466. session->newdata.eph,
  467. session->newdata.sep,
  468. session->newdata.epv,
  469. session->newdata.mode,
  470. session->newdata.status);
  471. break;
  472. case GARMIN_PKTID_RMD_DATA:
  473. case GARMIN_PKTID_RMD41_DATA:
  474. rmd = (cpo_rcv_data *) buf;
  475. GPSD_LOG(LOG_DATA, &session->context->errout,
  476. "Garmin: PVT RMD Data Sz: %d\n", pkt_len);
  477. GPSD_LOG(LOG_PROG, &session->context->errout,
  478. "Garmin: PVT RMD rcvr_tow: %f, rcvr_wn: %d\n",
  479. GPSD_LED64(rmd->rcvr_tow), GPSD_LE16TOH(rmd->rcvr_wn));
  480. for (i = 0; i < GARMIN_CHANNELS; i++) {
  481. GPSD_LOG(LOG_INF, &session->context->errout,
  482. "Garmin: PVT RMD Sat: %3u, cycles: %9u, pr: %16.6f, "
  483. "phase: %7.3f, slp_dtct: %3s, snr: %3u, Valid: %3s\n",
  484. (int)rmd->sv[i].svid + 1,
  485. GPSD_LE32TOH(rmd->sv[i].cycles),
  486. GPSD_LED64(rmd->sv[i].pr),
  487. (GPSD_LE16TOH(rmd->sv[i].phase) * 360.0) / 2048.0,
  488. rmd->sv[i].slp_dtct != 0 ? "Yes" : "No",
  489. rmd->sv[i].snr_dbhz,
  490. rmd->sv[i].valid != 0 ? "Yes" : "No");
  491. }
  492. break;
  493. case GARMIN_PKTID_SAT_DATA:
  494. // record ID 0x72 (114)
  495. GPSD_LOG(LOG_PROG, &session->context->errout,
  496. "Garmin: SAT Data Sz: %d\n", pkt_len);
  497. sats = (cpo_sat_data *) buf;
  498. session->gpsdata.satellites_used = 0;
  499. gpsd_zero_satellites(&session->gpsdata);
  500. for (i = 0, j = 0; i < GARMIN_CHANNELS; i++, sats++) {
  501. GPSD_LOG(LOG_INF, &session->context->errout,
  502. "Garmin: Sat %2d, snr: %5u, elev: %2d, Azmth: %3d, "
  503. "Stat: x%x\n",
  504. sats->svid, GPSD_LE16TOH(sats->snr), sats->elev,
  505. GPSD_LE16TOH(sats->azmth),
  506. sats->status);
  507. if (255 == (int)sats->svid) {
  508. // Garmin uses 255 for empty
  509. // gpsd uses 0 for empty
  510. continue;
  511. }
  512. if ((int)sats->svid <= 32) {
  513. /* GPS 1-32 */
  514. session->gpsdata.skyview[j].PRN = (short)sats->svid;
  515. session->gpsdata.skyview[j].svid = (short)sats->svid;
  516. session->gpsdata.skyview[j].gnssid = GNSSID_GPS;
  517. } else {
  518. /* SBAS 33-64 */
  519. session->gpsdata.skyview[j].PRN = (short)sats->svid;
  520. session->gpsdata.skyview[j].svid = (short)sats->svid + 87;
  521. session->gpsdata.skyview[j].gnssid = GNSSID_SBAS;
  522. }
  523. session->gpsdata.skyview[j].azimuth =
  524. (short)GPSD_LE16TOH(sats->azmth);
  525. session->gpsdata.skyview[j].elevation = (short)sats->elev;
  526. if (0xffff == sats->snr) {
  527. session->gpsdata.skyview[j].ss = NAN;
  528. } else {
  529. // Garmin does not document this. snr is in dB*100
  530. // Known, but not seen satellites have a dB value of -1*100
  531. session->gpsdata.skyview[j].ss =
  532. (float)(GPSD_LE16TOH(sats->snr) / 100.0);
  533. }
  534. // FIX-ME: Garmin documents this, but Daniel Dorau
  535. // <daniel.dorau@gmx.de> says the behavior on his GPSMap60CSX
  536. // doesn't match it.
  537. if ((uint8_t) 0 != (sats->status & 4)) {
  538. // used in solution?
  539. session->gpsdata.skyview[j].used = true;
  540. session->gpsdata.satellites_used++;
  541. }
  542. session->gpsdata.satellites_visible++;
  543. j++;
  544. }
  545. session->gpsdata.skyview_time.tv_sec = 0;
  546. session->gpsdata.skyview_time.tv_nsec = 0;
  547. mask |= USED_IS | SATELLITE_SET;
  548. GPSD_LOG(LOG_DATA, &session->context->errout,
  549. "Garmin: SAT_DATA: visible=%d used=%d\n",
  550. session->gpsdata.satellites_visible,
  551. session->gpsdata.satellites_used);
  552. break;
  553. case GARMIN_PKTID_PROTOCOL_ARRAY:
  554. // Pid_Protocol_Array, ID 253
  555. // this packet is never requested, it just comes, in some case
  556. // after a GARMIN_PKTID_PRODUCT_RQST
  557. GPSD_LOG(LOG_INF, &session->context->errout,
  558. "Garmin: Appl, Product Capability, sz: %d\n",
  559. pkt_len);
  560. for (i = 0; i < pkt_len; i += 3) {
  561. GPSD_LOG(LOG_INF, &session->context->errout,
  562. "Garmin: %c%03d\n",
  563. buf[i], get_uint16((uint8_t *) & buf[i + 1]));
  564. }
  565. break;
  566. default:
  567. GPSD_LOG(LOG_WARN, &session->context->errout,
  568. "Garmin: Unknown packet id: %#02x, Sz: %#02x\n",
  569. pkt_id, pkt_len);
  570. break;
  571. }
  572. GPSD_LOG(LOG_DATA, &session->context->errout,
  573. "Garmin: PrintSERPacket(, %#02x, %#02x, ) mask=(%s)\n",
  574. pkt_id, pkt_len, gps_maskdump(mask));
  575. return mask;
  576. }
  577. #if defined(HAVE_LIBUSB) && defined(__linux__)
  578. // This works around cppcheck not looking into enough config branches
  579. // cppcheck-suppress unusedFunction
  580. static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t * pkt)
  581. /* For debugging, decodes and prints some known packets */
  582. {
  583. gps_mask_t mask = 0;
  584. int maj_ver;
  585. int min_ver;
  586. uint32_t mode = 0;
  587. uint16_t prod_id = 0;
  588. uint32_t veri = 0;
  589. uint32_t serial;
  590. uint32_t mDataSize = get_int32((uint8_t *) & pkt->mDataSize);
  591. uint8_t *buffer = (uint8_t *) pkt;
  592. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: PrintUSBPacket()\n");
  593. if (DLE == pkt->mPacketType) {
  594. GPSD_LOG(LOG_PROG, &session->context->errout,
  595. "Garmin: really a SER packet!\n");
  596. return PrintSERPacket(session,
  597. (unsigned char)buffer[1],
  598. (int)buffer[2], (unsigned char *)(buffer + 3));
  599. }
  600. if (4096 < mDataSize) {
  601. GPSD_LOG(LOG_WARN, &session->context->errout,
  602. "Garmin: bogus packet, size too large=%d\n",
  603. mDataSize);
  604. return 0;
  605. }
  606. switch (pkt->mPacketType) {
  607. case GARMIN_LAYERID_TRANSPORT:
  608. /* Garmin USB layer specific */
  609. switch (pkt->mPacketId) {
  610. case GARMIN_PKTID_TRANSPORT_START_SESSION_REQ:
  611. // Pid_Start_Session, ID 5
  612. GPSD_LOG(LOG_PROG, &session->context->errout,
  613. "Garmin: Transport, Start Session req\n");
  614. break;
  615. case GARMIN_PKTID_TRANSPORT_START_SESSION_RESP:
  616. // Pid_Session_Started, ID 6
  617. mode = get_int32(&pkt->mData.uchars[0]);
  618. GPSD_LOG(LOG_PROG, &session->context->errout,
  619. "Garmin: Transport, Start Session resp, unit: 0x%x\n",
  620. mode);
  621. break;
  622. default:
  623. GPSD_LOG(LOG_PROG, &session->context->errout,
  624. "Garmin: Transport, Packet: Type %d %d %d, ID: %d,"
  625. "Sz: %d\n",
  626. pkt->mPacketType, pkt->mReserved1, pkt->mReserved2,
  627. pkt->mPacketId, mDataSize);
  628. break;
  629. }
  630. break;
  631. case GARMIN_LAYERID_APPL:
  632. /* raw data transport, shared with Garmin Serial Driver */
  633. mask = PrintSERPacket(session,
  634. (unsigned char)pkt->mPacketId,
  635. (int)mDataSize,
  636. (unsigned char *)pkt->mData.uchars);
  637. break;
  638. case 75:
  639. // private, garmin USB kernel driver specific
  640. switch (pkt->mPacketId) {
  641. case PRIV_PKTID_SET_MODE:
  642. prod_id = get_uint16(&pkt->mData.uchars[0]);
  643. GPSD_LOG(LOG_PROG, &session->context->errout,
  644. "Garmin: Private, Set Mode: %d\n", prod_id);
  645. break;
  646. case PRIV_PKTID_INFO_REQ:
  647. GPSD_LOG(LOG_PROG, &session->context->errout,
  648. "Garmin: Private, ID: Info Req\n");
  649. break;
  650. case PRIV_PKTID_INFO_RESP:
  651. veri = get_int32(pkt->mData.uchars);
  652. maj_ver = (int)(veri >> 16);
  653. min_ver = (int)(veri & 0xffff);
  654. mode = get_int32(&pkt->mData.uchars[4]);
  655. serial = get_int32(&pkt->mData.uchars[8]);
  656. GPSD_LOG(LOG_PROG, &session->context->errout,
  657. "Garmin: Private, ID: Info Resp\n");
  658. GPSD_LOG(LOG_INF, &session->context->errout,
  659. "Garmin: USB Driver found, Version %d.%d, Mode: %d, GPS Serial# %u\n",
  660. maj_ver, min_ver, mode, serial);
  661. break;
  662. default:
  663. GPSD_LOG(LOG_PROG, &session->context->errout,
  664. "Garmin: Private, Packet: ID: %d, Sz: %d\n",
  665. pkt->mPacketId, mDataSize);
  666. break;
  667. }
  668. break;
  669. default:
  670. GPSD_LOG(LOG_PROG, &session->context->errout,
  671. "Garmin: Packet: Type %d %d %d, ID: %d, Sz: %d\n",
  672. pkt->mPacketType, pkt->mReserved1, pkt->mReserved2,
  673. pkt->mPacketId, mDataSize);
  674. break;
  675. }
  676. return mask;
  677. }
  678. #endif /* HAVE_LIBUSB */
  679. #if defined(HAVE_LIBUSB) && defined(__linux__)
  680. /* build and send a packet w/ USB protocol */
  681. static void Build_Send_USB_Packet(struct gps_device_t *session,
  682. uint32_t layer_id, uint32_t pkt_id,
  683. uint32_t length, uint32_t data)
  684. {
  685. uint8_t *buffer = (uint8_t *) session->driver.garmin.Buffer;
  686. Packet_t *thePacket = (Packet_t *) buffer;
  687. ssize_t theBytesReturned = 0;
  688. ssize_t theBytesToWrite = 12 + (ssize_t) length;
  689. set_int32(buffer, layer_id);
  690. set_int32(buffer + 4, pkt_id);
  691. set_int32(buffer + 8, length);
  692. if (2 == length) {
  693. set_int16(buffer + 12, data);
  694. } else if (4 == length) {
  695. set_int32(buffer + 12, data);
  696. }
  697. (void)PrintUSBPacket(session, thePacket);
  698. theBytesReturned = gpsd_write(session, (const char *)thePacket,
  699. (size_t) theBytesToWrite);
  700. GPSD_LOG(LOG_PROG, &session->context->errout,
  701. "Garmin: SendPacket(), wrote %zd bytes\n",
  702. theBytesReturned);
  703. // Garmin says:
  704. // If the packet size was an exact multiple of the USB packet
  705. // size, we must make a final write call with no data
  706. // as a practical matter no known packets are 64 bytes long so
  707. // this is untested
  708. // So here goes just in case
  709. if (0 == (theBytesToWrite % ASYNC_DATA_SIZE)) {
  710. char *n = "";
  711. (void)gpsd_write(session, n, 0);
  712. }
  713. }
  714. #endif /* HAVE_LIBUSB && __linux__ */
  715. /* build and send a packet in serial protocol */
  716. /* layer_id unused */
  717. // FIX-ME: This should go through the common message buffer someday
  718. static void Build_Send_SER_Packet(struct gps_device_t *session,
  719. uint32_t layer_id UNUSED, uint32_t pkt_id,
  720. uint32_t length, uint32_t data)
  721. {
  722. uint8_t *buffer = (uint8_t *) session->driver.garmin.Buffer;
  723. uint8_t *buffer0 = buffer;
  724. Packet_t *thePacket = (Packet_t *) buffer;
  725. ssize_t theBytesReturned = 0;
  726. ssize_t theBytesToWrite = 6 + (ssize_t) length;
  727. uint8_t chksum = 0;
  728. *buffer++ = (uint8_t) DLE;
  729. *buffer++ = (uint8_t) pkt_id;
  730. chksum = pkt_id;
  731. *buffer++ = (uint8_t) length;
  732. chksum += length;
  733. /* ??? What is this doing? */
  734. if (2 == length) {
  735. /* careful! no DLE stuffing here! */
  736. set_int16(buffer, data);
  737. chksum += buffer[0];
  738. chksum += buffer[1];
  739. } else if (4 == length) {
  740. /* careful! no DLE stuffing here! */
  741. set_int32(buffer, data);
  742. chksum += buffer[0];
  743. chksum += buffer[1];
  744. chksum += buffer[2];
  745. chksum += buffer[3];
  746. }
  747. /* ??? How is data copied to the buffer? */
  748. buffer += length;
  749. // Add checksum
  750. *buffer++ = -chksum;
  751. if (DLE == -chksum) {
  752. /* stuff another DLE */
  753. *buffer++ = (uint8_t) DLE;
  754. theBytesToWrite++;
  755. }
  756. // Add DLE, ETX
  757. *buffer++ = (uint8_t) DLE;
  758. /* we used to say n++ here, but scan-build complains */
  759. *buffer = (uint8_t) ETX;
  760. (void)PrintSERPacket(session,
  761. (unsigned char)buffer0[1],
  762. (int)buffer0[2], (unsigned char *)(buffer0 + 3));
  763. theBytesReturned = gpsd_write(session, (const char *)thePacket,
  764. (size_t) theBytesToWrite);
  765. GPSD_LOG(LOG_PROG, &session->context->errout,
  766. "Garmin: SendPacket(), wrote %zd bytes\n",
  767. theBytesReturned);
  768. }
  769. #if defined(HAVE_LIBUSB) && defined(__linux__)
  770. /*
  771. * is_usb_device() - is a specified device USB matching given vendor/product?
  772. *
  773. * BUG: Doesn't actually match against path yet. Must finish this function
  774. * by querying /sys/dev/char, either directly or using libudev. Greg KH
  775. * assures this is possible, though he is vague about how.
  776. *
  777. * libudev: http://www.kernel.org/pub/linux/utils/kernel/hotplug/libudev/
  778. */
  779. // This works around cppcheck not looking into enough config branches
  780. // cppcheck-suppress unusedFunction
  781. static bool is_usb_device(const char *path UNUSED, int vendor, int product,
  782. struct gpsd_errout_t *errout)
  783. {
  784. // discover devices
  785. libusb_device **list;
  786. ssize_t cnt;
  787. ssize_t i = 0;
  788. bool found = false;
  789. GPSD_LOG(LOG_INF, errout, "attempting USB device enumeration.\n");
  790. (void)libusb_init(NULL);
  791. if ((cnt = libusb_get_device_list(NULL, &list)) < 0) {
  792. GPSD_LOG(LOG_ERROR, errout, "USB device list call failed.\n");
  793. libusb_exit(NULL);
  794. return false;
  795. }
  796. for (i = 0; i < cnt; i++) {
  797. struct libusb_device_descriptor desc;
  798. libusb_device *dev = list[i];
  799. int r = libusb_get_device_descriptor(dev, &desc);
  800. if (r < 0) {
  801. GPSD_LOG(LOG_ERROR, errout,
  802. "USB descriptor fetch failed on device %zd.\n", i);
  803. continue;
  804. }
  805. /* we can extract device descriptor data */
  806. GPSD_LOG(LOG_INF, errout,
  807. "%04x:%04x (bus %d, device %d)\n",
  808. desc.idVendor, desc.idProduct,
  809. libusb_get_bus_number(dev),
  810. libusb_get_device_address(dev));
  811. /* we match if vendor and product ID are right */
  812. if (desc.idVendor == (uint16_t)vendor && desc.idProduct == (uint16_t)product) {
  813. found = true;
  814. break;
  815. }
  816. }
  817. GPSD_LOG(LOG_INF, errout,
  818. "vendor/product match with %04x:%04x %sfound\n",
  819. vendor, product, found ? "" : "not ");
  820. libusb_free_device_list(list, 1);
  821. libusb_exit(NULL);
  822. return found;
  823. }
  824. #endif /* HAVE_LIBUSB */
  825. /*
  826. * garmin_usb_detect() - detect a Garmin USB device connected to session fd.
  827. *
  828. * This is ONLY for USB devices reporting as: 091e:0003.
  829. *
  830. * This driver ONLY works in Linux and ONLY when the garmin_gps kernel
  831. * module is installed.
  832. *
  833. * This is only necessary because under Linux Garmin USB devices need a
  834. * kernel module rather than being normal USB-serial devices.
  835. *
  836. * The actual wire protocol from the Garmin device is very strange. There
  837. * are no delimiters. End of packet is signaled by a zero-length read
  838. * on the USB device, and start of packet is the next read. You can't just
  839. * ignore the zero reads and pass the data through - you'd never be able
  840. * to tell where the packet boundaries are.
  841. *
  842. * The garmin_usb module's job is to grab the packet and frame it in
  843. * DLEs (with DLE stuffing). This makes the USB packets look as
  844. * though they came from a regular Garmin *serial* device, which is how
  845. * most of the processing for both types can be unified here.
  846. *
  847. * return 1 is device found
  848. * return 0 if not
  849. */
  850. static bool garmin_usb_detect(struct gps_device_t *session UNUSED)
  851. {
  852. #if defined(__linux__)
  853. /*
  854. * Only perform this check if we're looking at a USB-serial
  855. * device. This prevents drivers for attached serial GPSes
  856. * fronm being rudely elbowed aside by this one if they happen
  857. * to be trying to coexist with the Garmin.
  858. */
  859. if (session->sourcetype != source_usb)
  860. return false;
  861. else {
  862. #ifdef HAVE_LIBUSB
  863. if (!is_usb_device(session->gpsdata.dev.path, 0x091e, 0x0003,
  864. &session->context->errout))
  865. return false;
  866. if (!gpsd_set_raw(session)) {
  867. GPSD_LOG(LOG_ERROR, &session->context->errout,
  868. "Garmin: garmin_usb_detect: error changing port attributes: %s\n",
  869. strerror(errno));
  870. return false;
  871. }
  872. if (sizeof(session->driver.garmin.Buffer) < sizeof(Packet_t)) {
  873. /* dunno how this happens, but it does on some compilers */
  874. GPSD_LOG(LOG_ERROR, &session->context->errout,
  875. "Garmin: garmin_usb_detect: Compile error, garmin.Buffer too small.\n");
  876. return false;
  877. }
  878. // FIXME!!! needs to use libusb totally and move garmin_gps aside */
  879. // set Mode 1, mode 0 is broken somewhere past 2.6.14
  880. // but how?
  881. GPSD_LOG(LOG_PROG, &session->context->errout,
  882. "Garmin: Set garmin_gps driver mode = 0\n");
  883. Build_Send_USB_Packet(session, GARMIN_LAYERID_PRIVATE,
  884. PRIV_PKTID_SET_MODE, 4, MODE_GARMIN_SERIAL);
  885. // expect no return packet !?
  886. return true;
  887. #else
  888. return false;
  889. #endif /* HAVE_LIBUSB */
  890. }
  891. #else
  892. return false;
  893. #endif /* __linux__ */
  894. }
  895. static void garmin_event_hook(struct gps_device_t *session, event_t event)
  896. {
  897. if (session->context->readonly ||
  898. session->context->passive) {
  899. return;
  900. }
  901. /*
  902. * FIX-ME: It might not be necessary to call this on reactivate.
  903. * Experiment to see if the holds its settings through a close.
  904. */
  905. if (event == event_identified || event == event_reactivate) {
  906. // Tell the device to send product data
  907. GPSD_LOG(LOG_PROG, &session->context->errout,
  908. "Garmin: Get Product Data\n");
  909. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  910. GARMIN_PKTID_PRODUCT_RQST, 0, 0);
  911. // turn on PVT data 49
  912. GPSD_LOG(LOG_PROG, &session->context->errout,
  913. "Garmin: Set to send reports every 1 second\n");
  914. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  915. GARMIN_PKTID_L001_COMMAND_DATA, 2,
  916. CMND_START_PVT_DATA);
  917. #if USE_RMD
  918. // turn on RMD data 110
  919. GPSD_LOG(LOG_PROG, &session->context->errout,
  920. "Garmin: Set to send Raw sat data\n");
  921. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  922. GARMIN_PKTID_L001_COMMAND_DATA, 2,
  923. CMND_START_RM_DATA);
  924. #endif
  925. }
  926. if (event == event_deactivate)
  927. /* FIX-ME: is any action needed, or is closing the port sufficient? */
  928. GPSD_LOG(LOG_PROG, &session->context->errout,
  929. "Garmin: garmin_close()\n");
  930. }
  931. #define Send_ACK() Build_Send_SER_Packet(session, 0, ACK, 0, 0)
  932. #define Send_NAK() Build_Send_SER_Packet(session, 0, NAK, 0, 0)
  933. gps_mask_t garmin_ser_parse(struct gps_device_t *session)
  934. {
  935. unsigned char *buf = session->lexer.outbuffer;
  936. size_t len = session->lexer.outbuflen;
  937. unsigned char data_buf[MAX_BUFFER_SIZE];
  938. unsigned char c;
  939. int i = 0;
  940. size_t n = 0;
  941. int data_index = 0;
  942. int got_dle = 0;
  943. unsigned char pkt_id = 0;
  944. unsigned char pkt_len = 0;
  945. unsigned char chksum = 0;
  946. gps_mask_t mask = 0;
  947. struct timespec delay;
  948. GPSD_LOG(LOG_RAW, &session->context->errout,
  949. "Garmin: garmin_ser_parse()\n");
  950. if (6 > len) {
  951. /* WTF? */
  952. /* minimum packet; <DLE> [pkt id] [length=0] [chksum] <DLE> <STX> */
  953. Send_NAK();
  954. GPSD_LOG(LOG_RAW, &session->context->errout,
  955. "Garmin: serial too short: %zd\n", len);
  956. return 0;
  957. }
  958. /* debug */
  959. for (i = 0; i < (int)len; i++) {
  960. GPSD_LOG(LOG_RAW, &session->context->errout,
  961. "Garmin: Char: %#02x\n", buf[i]);
  962. }
  963. if ('\x10' != buf[0]) {
  964. Send_NAK();
  965. GPSD_LOG(LOG_RAW, &session->context->errout,
  966. "Garmin: buf[0] not DLE\n");
  967. return 0;
  968. }
  969. n = 1;
  970. pkt_id = buf[n++];
  971. chksum = pkt_id;
  972. if ('\x10' == pkt_id) {
  973. if ('\x10' != buf[n++]) {
  974. Send_NAK();
  975. GPSD_LOG(LOG_RAW, &session->context->errout,
  976. "Garmin: Bad pkt_id %#02x\n", pkt_id);
  977. return 0;
  978. }
  979. }
  980. pkt_len = buf[n++];
  981. chksum += pkt_len;
  982. if ('\x10' == pkt_len) {
  983. if ('\x10' != buf[n++]) {
  984. GPSD_LOG(LOG_RAW, &session->context->errout,
  985. "Garmin: Bad pkt_len %#02x\n", pkt_len);
  986. Send_NAK();
  987. return 0;
  988. }
  989. }
  990. data_index = 0;
  991. for (i = 0; i < 256; i++) {
  992. if ((int)pkt_len == data_index) {
  993. // got it all
  994. break;
  995. }
  996. if (len < n + i) {
  997. GPSD_LOG(LOG_RAW, &session->context->errout,
  998. "Garmin: Packet too short %zd < %zd\n",
  999. len, n + i);
  1000. Send_NAK();
  1001. return 0;
  1002. }
  1003. c = buf[n + i];
  1004. if (got_dle) {
  1005. got_dle = 0;
  1006. if ('\x10' != c) {
  1007. Send_NAK();
  1008. GPSD_LOG(LOG_RAW, &session->context->errout,
  1009. "Garmin: Bad DLE %#02x\n", c);
  1010. return 0;
  1011. }
  1012. } else {
  1013. chksum += c;
  1014. data_buf[data_index++] = c;
  1015. if ('\x10' == c) {
  1016. got_dle = 1;
  1017. }
  1018. }
  1019. }
  1020. /* get checksum */
  1021. if (len < n + i) {
  1022. Send_NAK();
  1023. GPSD_LOG(LOG_RAW, &session->context->errout,
  1024. "Garmin: No checksum, Packet too short %zd < %zd\n", len,
  1025. n + i);
  1026. return 0;
  1027. }
  1028. c = buf[n + i++];
  1029. chksum += c;
  1030. /* get final DLE */
  1031. if (len < n + i) {
  1032. Send_NAK();
  1033. GPSD_LOG(LOG_RAW, &session->context->errout,
  1034. "Garmin: No final DLE, Packet too short %zd < %zd\n", len,
  1035. n + i);
  1036. return 0;
  1037. }
  1038. c = buf[n + i++];
  1039. if ('\x10' != c) {
  1040. Send_NAK();
  1041. GPSD_LOG(LOG_RAW, &session->context->errout,
  1042. "Garmin: Final DLE not DLE\n");
  1043. return 0;
  1044. }
  1045. /* get final ETX */
  1046. if (len < n + i) {
  1047. Send_NAK();
  1048. GPSD_LOG(LOG_RAW, &session->context->errout,
  1049. "Garmin: No final ETX, Packet too short %zd < %zd\n", len,
  1050. n + i);
  1051. return 0;
  1052. }
  1053. /* we used to say n++ here, but scan-build complains */
  1054. c = buf[n + i];
  1055. if ('\x03' != c) {
  1056. Send_NAK();
  1057. GPSD_LOG(LOG_RAW, &session->context->errout,
  1058. "Garmin: Final ETX not ETX\n");
  1059. return 0;
  1060. }
  1061. /* debug */
  1062. for (i = 0; i < data_index; i++) {
  1063. GPSD_LOG(LOG_RAW, &session->context->errout,
  1064. "Garmin: Char: %#02x\n", data_buf[i]);
  1065. }
  1066. GPSD_LOG(LOG_DATA, &session->context->errout,
  1067. "Garmin: garmin_ser_parse() Type: %#02x, Len: %#02x, chksum: %#02x\n",
  1068. pkt_id, pkt_len, chksum);
  1069. mask = PrintSERPacket(session, pkt_id, pkt_len, data_buf);
  1070. // sending ACK too soon might hang the session
  1071. // so send ACK last, after a pause
  1072. /* wait 300 uSec */
  1073. delay.tv_sec = 0;
  1074. delay.tv_nsec = 300000L;
  1075. nanosleep(&delay, NULL);
  1076. Send_ACK();
  1077. GPSD_LOG(LOG_DATA, &session->context->errout,
  1078. "Garmin: garmin_ser_parse( )\n");
  1079. return mask;
  1080. }
  1081. static void settle(void)
  1082. {
  1083. struct timespec delay, rem;
  1084. memset(&delay, 0, sizeof(delay));
  1085. delay.tv_sec = 0;
  1086. delay.tv_nsec = 333000000L;
  1087. nanosleep(&delay, &rem);
  1088. }
  1089. static void garmin_switcher(struct gps_device_t *session, int mode)
  1090. {
  1091. if (mode == MODE_NMEA) {
  1092. const unsigned char switcher[] =
  1093. { 0x10, 0x0A, 0x02, 0x26, 0x00, 0xCE, 0x10, 0x03 };
  1094. // Note hard-coded string length in the next line...
  1095. ssize_t status = gpsd_write(session, (char *)switcher,
  1096. sizeof(switcher));
  1097. if (status == (ssize_t)sizeof(switcher)) {
  1098. GPSD_LOG(LOG_PROG, &session->context->errout,
  1099. "Garmin: => GPS: turn off binary %02x %02x %02x... \n",
  1100. switcher[0], switcher[1], switcher[2]);
  1101. } else {
  1102. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1103. "Garmin: => GPS: FAILED\n");
  1104. }
  1105. settle(); // wait 333mS, essential!
  1106. /* once a sec, no binary, no averaging, NMEA 2.3, WAAS */
  1107. (void)nmea_send(session, "$PGRMC1,1,1");
  1108. //(void)nmea_send(fd, "$PGRMC1,1,1,1,,,,2,W,N");
  1109. (void)nmea_send(session, "$PGRMI,,,,,,,R");
  1110. settle(); // wait 333mS, essential!
  1111. } else {
  1112. (void)nmea_send(session, "$PGRMC1,1,2,1,,,,2,W,N");
  1113. (void)nmea_send(session, "$PGRMI,,,,,,,R");
  1114. settle(); // wait 333mS, essential!
  1115. }
  1116. }
  1117. static ssize_t garmin_control_send(struct gps_device_t *session,
  1118. char *buf, size_t buflen)
  1119. /* not used by the daemon, it's for gpsctl and friends */
  1120. {
  1121. session->msgbuflen = buflen;
  1122. (void)memcpy(session->msgbuf, buf, buflen);
  1123. return gpsd_write(session, session->msgbuf, session->msgbuflen);
  1124. }
  1125. static double garmin_time_offset(struct gps_device_t *session)
  1126. {
  1127. if (session->sourcetype == source_usb) {
  1128. return 0.035; /* Garmin USB, expect +/- 40mS jitter */
  1129. }
  1130. /* only two sentences ships time */
  1131. /* but the PVT data is always first */
  1132. switch (session->gpsdata.dev.baudrate) {
  1133. case 4800:
  1134. return 0.430; /* TBD */
  1135. case 9600:
  1136. return 0.430; /* tested 12Arp10 */
  1137. case 19200:
  1138. return 0.430; /* TBD */
  1139. case 38400:
  1140. return 0.430; /* TBD */
  1141. }
  1142. return 0.430; /* WTF? WAG */
  1143. }
  1144. /* this is everything we export */
  1145. /* *INDENT-OFF* */
  1146. const struct gps_type_t driver_garmin_usb_binary =
  1147. {
  1148. .type_name = "Garmin USB binary", /* full name of type */
  1149. .packet_type = GARMIN_PACKET, /* associated lexer packet type */
  1150. .flags = DRIVER_STICKY, /* remember this */
  1151. .trigger = NULL, /* no trigger, it has a probe */
  1152. .channels = GARMIN_CHANNELS, /* consumer-grade GPS */
  1153. .probe_detect = garmin_usb_detect,/* how to detect at startup time */
  1154. .get_packet = generic_get, /* how to grab a packet */
  1155. .parse_packet = garmin_ser_parse, /* parse message packets */
  1156. .rtcm_writer = NULL, /* don't send DGPS corrections */
  1157. .init_query = NULL, /* non-perturbing initial query */
  1158. .event_hook = garmin_event_hook,/* lifetime ebent handler */
  1159. .speed_switcher = NULL, /* no speed switcher */
  1160. .mode_switcher = NULL, /* Garmin USB Binary has no NMEA */
  1161. .rate_switcher = NULL, /* no sample-rate switcher */
  1162. .min_cycle.tv_sec = 0,
  1163. .min_cycle.tv_nsec = 10000000, /* 10Hz */
  1164. .control_send = garmin_control_send, /* send raw bytes */
  1165. .time_offset = garmin_time_offset,
  1166. };
  1167. /* *INDENT-ON* */
  1168. /* *INDENT-OFF* */
  1169. const struct gps_type_t driver_garmin_ser_binary =
  1170. {
  1171. .type_name = "Garmin Serial binary", /* full name of type */
  1172. .packet_type = GARMIN_PACKET, /* associated lexer packet type */
  1173. .flags = DRIVER_STICKY, /* remember this */
  1174. .trigger = NULL, /* no trigger, it has a probe */
  1175. .channels = GARMIN_CHANNELS, /* consumer-grade GPS */
  1176. .probe_detect = NULL, /* how to detect at startup time */
  1177. .get_packet = generic_get, /* how to grab a packet */
  1178. .parse_packet = garmin_ser_parse, /* parse message packets */
  1179. .rtcm_writer = NULL, /* don't send DGPS corrections */
  1180. .init_query = NULL, /* non-perturbing initial query */
  1181. /* The Garmin Gek0 301 needs to be kicked to start sending binary.
  1182. * like the Garmin USB. */
  1183. .event_hook = garmin_event_hook, /* lifetime event handler */
  1184. .speed_switcher = NULL, /* no speed switcher */
  1185. .mode_switcher = garmin_switcher, /* how to change modes */
  1186. .rate_switcher = NULL, /* no sample-rate switcher */
  1187. .min_cycle.tv_sec = 0,
  1188. .min_cycle.tv_nsec = 10000000, /* 10Hz */
  1189. .control_send = garmin_control_send, /* send raw bytes */
  1190. .time_offset = garmin_time_offset,
  1191. };
  1192. /* *INDENT-ON* */
  1193. #endif /* GARMIN_ENABLE */
  1194. // vim: set expandtab shiftwidth=4