| --- libmsc/rrlp.c Mon Jul 18 11:19:21 2011 |
| +++ r:libmsc/rrlp.c Wed Aug 10 07:34:26 2011 |
| @@ -20,37 +20,317 @@ |
| */ |
| |
| |
| #include <sys/types.h> |
| |
| #include <openbsc/gsm_04_08.h> |
| #include <openbsc/signal.h> |
| #include <openbsc/gsm_subscriber.h> |
| #include <openbsc/chan_alloc.h> |
| |
| +/* ----------------------------------------------- */ |
| + |
| +/* TODO: move in a separate file ? */ |
| + |
| +#include <stdio.h> |
| +#include <stdlib.h> |
| +#include <unistd.h> |
| +#include <errno.h> |
| +#include <sys/types.h> |
| +#include <sys/socket.h> |
| +#include <netinet/in.h> |
| +#include <arpa/inet.h> |
| + |
| +#define RRLP_SERV_PORT 7890 |
| +#define RRLP_SERV_IP "192.168.5.250" /* TODO: from config file */ |
| + |
| +#define MAX_RRLP_DATA 256 |
| + |
| +/* Server cmds */ |
| + |
| +#define RRLP_CMD_MS_DATA 1 /* data from MS */ |
| +#define RRLP_CMD_MS_DATA_SLOW 2 /* data from MS, slow channel */ |
| + |
| +/* Server response */ |
| + |
| +#define RRLP_RSP_ASSIST_DATA 1 /* assitance data, send to MS */ |
| +#define RRLP_RSP_RRLP_ERROR 2 /* RRLP error */ |
| +#define RRLP_RSP_RRLP_POSITION 3 /* RRLP position */ |
| +#define RRLP_RSP_ERROR 4 /* something went wrong */ |
| + |
| +/* TODO: adjust error messages, use logging */ |
| + |
| +static int rrlp_serv_cmd(struct gsm_subscriber_connection *conn, |
| + uint8_t cmd, uint8_t *data, int len_data, |
| + uint8_t *cmd_reply, uint8_t *reply, int *len_reply) |
| +{ |
| + static int fd = -1; |
| + static struct sockaddr_in sa; |
| + int len; |
| + uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */ |
| + int len_pkt, offs; |
| + int rc; |
| + long long unsigned int id; |
| + struct sockaddr_in from; |
| + int from_len; |
| + fd_set readset; |
| + struct timeval tv; |
| + |
| + if(len_data > MAX_RRLP_DATA) { |
| + fprintf(stderr, "len_data > MAX_RRLP_DATA: %d\n", len_data); |
| + return -1; |
| + } |
| + if(fd == -1) { |
| + fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); |
| + if(fd < 0) { |
| + fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno)); |
| + return -1; |
| + } |
| + |
| + sa.sin_family = AF_INET; |
| + sa.sin_port = htons(RRLP_SERV_PORT); |
| + if(inet_aton(RRLP_SERV_IP, &sa.sin_addr) != 1) { |
| + fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno)); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + |
| + rc = connect(fd, (struct sockaddr *)&sa, sizeof(sa)); |
| + if(rc < 0) { |
| + fprintf(stderr, "connect() failed: (%d) %s\n", rc, strerror(errno)); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + } |
| + |
| + /* we are now connected */ |
| + |
| + id = conn->subscr->id; |
| + |
| + /* build cmd packet */ |
| + |
| + len_pkt = 2 + 1 + 8 + len_data; |
| + buf[0] = len_pkt & 0xFF; |
| + buf[1] = (len_pkt >> 8) & 0xFF; |
| + |
| + buf[2] = cmd; |
| + |
| + buf[3] = id & 0xFF; |
| + buf[4] = (id >> 8) & 0xFF; |
| + buf[5] = (id >> 16) & 0xFF; |
| + buf[6] = (id >> 24) & 0xFF; |
| + buf[7] = (id >> 32) & 0xFF; |
| + buf[8] = (id >> 40) & 0xFF; |
| + buf[9] = (id >> 48) & 0xFF; |
| + buf[10] = (id >> 56) & 0xFF; |
| + /* data */ |
| + memcpy(&buf[11], data, len_data); |
| + |
| + /* send cmd */ |
| + |
| + len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&sa, sizeof(sa)); |
| + if(len < 0) { |
| + fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno)); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + if(len != len_pkt) { |
| + fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + |
| + /* wait at most 500 ms for a reply */ |
| + |
| + FD_ZERO(&readset); |
| + FD_SET(fd, &readset); |
| + tv.tv_sec = 0; |
| + tv.tv_usec = 500 * 1000; |
| + |
| + /* this creates another UDP socket on Cygwin !? */ |
| + rc = select(fd + 1, &readset, NULL, NULL, &tv); |
| + if(rc < 0) { |
| + fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + |
| + if(!FD_ISSET(fd, &readset)) { |
| + fprintf(stderr, "timeout select()\n"); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + |
| + /* read packet */ |
| + from_len = sizeof(from); |
| + len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len); |
| + if(len < 0) { |
| + fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno)); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + if(len < 2) { |
| + fprintf(stderr, "len < 2: %d\n", len); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + len_pkt = buf[0] + (buf[1] << 8); |
| + if(len_pkt < 2 + 1) { |
| + fprintf(stderr, "len_pkt < 2 + 1: %d\n", len_pkt); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + if(len != len_pkt) { |
| + fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt); |
| + close(fd); |
| + fd = -1; |
| + return -1; |
| + } |
| + len_pkt -= 2; |
| + offs = 2; |
| + |
| +#if 0 /* dump packet */ |
| + { |
| + int i; |
| + for(i = 0; i < len_pkt; i++) |
| + printf("%02X ", buf[offs + i]); |
| + printf("\n"); |
| + } |
| +#endif |
| + |
| + /* process packet */ |
| + |
| + *len_reply = len_pkt - 1; |
| + *cmd_reply = buf[offs]; |
| + memcpy(reply, &buf[offs + 1], *len_reply); |
| + |
| + return 0; |
| +} |
| + |
| +/* ----------------------------------------------- */ |
| + |
| +static int send_rrlp_req(struct gsm_subscriber_connection *conn); |
| + |
| +/* TODO: adjust error messages, use logging */ |
| + |
| +int handle_rrlp(struct gsm_subscriber_connection *conn, uint8_t *data, int len) |
| +{ |
| + struct gsm_network *net = conn->bts->network; |
| + int rc; |
| + uint8_t cmd_reply; |
| + uint8_t reply[MAX_RRLP_DATA]; |
| + int len_reply; |
| + uint8_t cmd; |
| + |
| + if (net->rrlp.mode == RRLP_MODE_NONE) |
| + return 0; |
| + |
| + if(len > MAX_RRLP_DATA) { |
| + fprintf(stderr, "too many data for handle_rrlp (%d)\n", len); |
| + return -1; |
| + } |
| + |
| + /* TODO: decide if channel is slow (SDCCH), for slow channels |
| + only short assistance data should be sent */ |
| + |
| + if(1) |
| + cmd = RRLP_CMD_MS_DATA; |
| + else |
| + cmd = RRLP_CMD_MS_DATA_SLOW; |
| + |
| + rc = rrlp_serv_cmd(conn, cmd, data, len, &cmd_reply, reply, &len_reply); |
| + if(rc != 0) { |
| + fprintf(stderr, "rrlp_serv_cmd failed (%d)\n", rc); |
| + return rc; |
| + } |
| + |
| + if(cmd_reply == RRLP_RSP_ERROR) { |
| + printf("RRLP Server error (general): %s\n", reply); |
| + return 0; |
| + } |
| + |
| + if(cmd_reply == RRLP_RSP_RRLP_ERROR) { |
| + printf("RRLP Server error (RRLP): %s\n", reply); |
| + return 0; |
| + } |
| + |
| + if(cmd_reply == RRLP_RSP_RRLP_POSITION) { |
| + long latitude; |
| + long longitude; |
| + long altitude; |
| + |
| + if(len_reply != 12) { |
| + fprintf(stderr, "invalid RRLP position length (%d)\n", len_reply); |
| + return -1; |
| + } |
| + |
| + latitude = reply[0] + (reply[1] << 8) + (reply[2] << 16) + (reply[3] << 24); |
| + longitude = reply[4] + (reply[5] << 8) + (reply[6] << 16) + (reply[7] << 24); |
| + altitude = reply[8] + (reply[9] << 8) + (reply[10] << 16) + (reply[11] << 24); |
| + |
| + /* TODO: do something useful with the position */ |
| + |
| + printf("RRLP Server position: "); |
| + printf("latitude = %f ", ((double)latitude * 90.0) / 0x800000L); |
| + printf("longitude = %f ", ((double)longitude * 360.0) / 0x1000000L); |
| + printf("altitude = %ld\n", altitude); |
| + |
| + return 0; |
| + } |
| + |
| + if(cmd_reply == RRLP_RSP_ASSIST_DATA) { |
| + printf("Assistance data, len %d\n", len_reply); |
| + |
| + /* |
| + If there are assistance data, send them. If there are no more, |
| + repeat the measurement request |
| + */ |
| + |
| + if(len_reply) |
| + return gsm48_send_rr_app_info(conn, 0x00, len_reply, reply); |
| + else |
| + send_rrlp_req(conn); |
| + } |
| + |
| + return 0; |
| +} |
| + |