Early/Obsolete code for Location Service (LCS / SMLC / RRLP)
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
osmocom-lcs/ubx.c

96 lines
2.4 KiB

/*
* ubx.c
*
* Implementation of generic UBX helpers
*
*
* Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdint.h>
#include "ubx.h"
static void
ubx_checksum(uint8_t *data, int len, uint8_t *cksum)
{
int i;
uint8_t ck0 = 0, ck1 = 0;
for (i=0; i<len; i++) {
ck0 += data[i];
ck1 += ck0;
}
cksum[0] = ck0;
cksum[1] = ck1;
}
static ubx_msg_handler_t
ubx_find_handler(struct ubx_dispatch_entry *dt, uint8_t msg_class, uint8_t msg_id)
{
while (dt->handler) {
if ((dt->msg_class == msg_class) && (dt->msg_id == msg_id))
return dt->handler;
dt++;
}
return NULL;
}
int
ubx_msg_dispatch(struct ubx_dispatch_entry *dt,
void *msg, int len, void *userdata)
{
struct ubx_hdr *hdr = msg;
uint8_t cksum[2], *cksum_ptr;
ubx_msg_handler_t h;
if (len < 2) {
fprintf(stderr, "[!] Length too small (%d)\n", len);
return -1;
}
if ((hdr->sync[0] != UBX_SYNC0) || (hdr->sync[1] != UBX_SYNC1)) {
fprintf(stderr, "[!] Invalid sync bytes\n");
return -1;
}
if (len < sizeof(struct ubx_hdr)) {
fprintf(stderr, "[!] Length too small for UBX header (%d)\n", len);
return -1;
}
if (len < sizeof(struct ubx_hdr) + hdr->payload_len - 2) {
fprintf(stderr, "[!] Length too small for UBX header and payload (%d)\n", len);
return -1;
}
ubx_checksum(msg + 2, sizeof(struct ubx_hdr) + hdr->payload_len - 2, cksum);
cksum_ptr = msg + (sizeof(struct ubx_hdr) + hdr->payload_len);
if ((cksum_ptr[0] != cksum[0]) || (cksum_ptr[1] != cksum[1])) {
fprintf(stderr, "[!] Invalid checksum\n");
return -1;
}
h = ubx_find_handler(dt, hdr->msg_class, hdr->msg_id);
if (h)
h(hdr, msg + sizeof(struct ubx_hdr), hdr->payload_len, userdata);
return sizeof(struct ubx_hdr) + hdr->payload_len + 2;
}