Sylvain Munaut | ce7be3a | 2009-12-22 21:53:22 +0100 | [diff] [blame] | 1 | /* |
| 2 | * ubx.c |
| 3 | * |
| 4 | * Implementation of generic UBX helpers |
| 5 | * |
| 6 | * |
| 7 | * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com> |
| 8 | * |
| 9 | * This program is free software: you can redistribute it and/or modify |
| 10 | * it under the terms of the GNU General Public License as published by |
| 11 | * the Free Software Foundation, either version 2 of the License, or |
| 12 | * (at your option) any later version. |
| 13 | * |
| 14 | * This program is distributed in the hope that it will be useful, |
| 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 17 | * GNU General Public License for more details. |
| 18 | * |
| 19 | * You should have received a copy of the GNU General Public License |
| 20 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 21 | */ |
| 22 | |
| 23 | #include <stdio.h> |
| 24 | #include <stdint.h> |
| 25 | |
| 26 | #include "ubx.h" |
| 27 | |
| 28 | |
| 29 | static void |
| 30 | ubx_checksum(uint8_t *data, int len, uint8_t *cksum) |
| 31 | { |
| 32 | int i; |
| 33 | uint8_t ck0 = 0, ck1 = 0; |
| 34 | for (i=0; i<len; i++) { |
| 35 | ck0 += data[i]; |
| 36 | ck1 += ck0; |
| 37 | } |
| 38 | cksum[0] = ck0; |
| 39 | cksum[1] = ck1; |
| 40 | } |
| 41 | |
| 42 | |
| 43 | static ubx_msg_handler_t |
| 44 | ubx_find_handler(struct ubx_dispatch_entry *dt, uint8_t msg_class, uint8_t msg_id) |
| 45 | { |
| 46 | while (dt->handler) { |
| 47 | if ((dt->msg_class == msg_class) && (dt->msg_id == msg_id)) |
| 48 | return dt->handler; |
| 49 | dt++; |
| 50 | } |
| 51 | return NULL; |
| 52 | } |
| 53 | |
| 54 | |
| 55 | int |
| 56 | ubx_msg_dispatch(struct ubx_dispatch_entry *dt, |
| 57 | void *msg, int len, void *userdata) |
| 58 | { |
| 59 | struct ubx_hdr *hdr = msg; |
| 60 | uint8_t cksum[2], *cksum_ptr; |
| 61 | ubx_msg_handler_t h; |
| 62 | |
| 63 | if ((hdr->sync[0] != UBX_SYNC0) || (hdr->sync[1] != UBX_SYNC1)) { |
| 64 | fprintf(stderr, "[!] Invalid sync bytes\n"); |
| 65 | return -1; |
| 66 | } |
| 67 | |
| 68 | ubx_checksum(msg + 2, sizeof(struct ubx_hdr) + hdr->payload_len - 2, cksum); |
| 69 | cksum_ptr = msg + (sizeof(struct ubx_hdr) + hdr->payload_len); |
| 70 | if ((cksum_ptr[0] != cksum[0]) || (cksum_ptr[1] != cksum[1])) { |
| 71 | fprintf(stderr, "[!] Invalid checksum\n"); |
| 72 | return -1; |
| 73 | } |
| 74 | |
| 75 | h = ubx_find_handler(dt, hdr->msg_class, hdr->msg_id); |
| 76 | if (h) |
| 77 | h(hdr, msg + sizeof(struct ubx_hdr), hdr->payload_len, userdata); |
| 78 | |
| 79 | return sizeof(struct ubx_hdr) + hdr->payload_len + 2; |
| 80 | } |
| 81 | |