src/gsmtap_util.c

   1 /* GSMTAP support code in libmsomcore */
   2 /*
   3  * (C) 2010-2011 by Harald Welte <laforge@gnumonks.org>
   4  *
   5  * All Rights Reserved
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  20  *
  21  */
  22
  23 #include "../config.h"
  24
  25 #include <osmocom/core/gsmtap_util.h>
  26 #include <osmocom/core/logging.h>
  27 #include <osmocom/core/gsmtap.h>
  28 #include <osmocom/core/msgb.h>
  29 #include <osmocom/core/talloc.h>
  30 #include <osmocom/core/select.h>
  31 #include <osmocom/core/socket.h>
  32 #include <osmocom/gsm/protocol/gsm_04_08.h>
  33 #include <osmocom/gsm/rsl.h>
  34
  35 #include <sys/types.h>
  36
  37 #include <arpa/inet.h>
  38
  39 #include <stdio.h>
  40 #include <unistd.h>
  41 #include <stdint.h>
  42 #include <string.h>
  43 #include <errno.h>
  44
  45 uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id)
  46 {
  47         uint8_t ret = GSMTAP_CHANNEL_UNKNOWN;
  48
  49         switch (rsl_chantype) {
  50         case RSL_CHAN_Bm_ACCHs:
  51                 ret = GSMTAP_CHANNEL_TCH_F;
  52                 break;
  53         case RSL_CHAN_Lm_ACCHs:
  54                 ret = GSMTAP_CHANNEL_TCH_H;
  55                 break;
  56         case RSL_CHAN_SDCCH4_ACCH:
  57                 ret = GSMTAP_CHANNEL_SDCCH4;
  58                 break;
  59         case RSL_CHAN_SDCCH8_ACCH:
  60                 ret = GSMTAP_CHANNEL_SDCCH8;
  61                 break;
  62         case RSL_CHAN_BCCH:
  63                 ret = GSMTAP_CHANNEL_BCCH;
  64                 break;
  65         case RSL_CHAN_RACH:
  66                 ret = GSMTAP_CHANNEL_RACH;
  67                 break;
  68         case RSL_CHAN_PCH_AGCH:
  69                 /* it could also be AGCH... */
  70                 ret = GSMTAP_CHANNEL_PCH;
  71                 break;
  72         }
  73
  74         if (link_id & 0x40)
  75                 ret |= GSMTAP_CHANNEL_ACCH;
  76
  77         return ret;
  78 }
  79
  80 /* receive a message from L1/L2 and put it in GSMTAP */
  81 struct msgb *gsmtap_makemsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type,
  82                             uint8_t ss, uint32_t fn, int8_t signal_dbm,
  83                             uint8_t snr, const uint8_t *data, unsigned int len)
  84 {
  85         struct msgb *msg;
  86         struct gsmtap_hdr *gh;
  87         uint8_t *dst;
  88
  89         msg = msgb_alloc(sizeof(*gh) + len, "gsmtap_tx");
  90         if (!msg)
  91                 return NULL;
  92
  93         gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh));
  94
  95         gh->version = GSMTAP_VERSION;
  96         gh->hdr_len = sizeof(*gh)/4;
  97         gh->type = GSMTAP_TYPE_UM;
  98         gh->timeslot = ts;
  99         gh->sub_slot = ss;
 100         gh->arfcn = htons(arfcn);
 101         gh->snr_db = snr;
 102         gh->signal_dbm = signal_dbm;
 103         gh->frame_number = htonl(fn);
 104         gh->sub_type = chan_type;
 105         gh->antenna_nr = 0;
 106
 107         dst = msgb_put(msg, len);
 108         memcpy(dst, data, len);
 109
 110         return msg;
 111 }
 112
 113 #ifdef HAVE_SYS_SOCKET_H
 114
 115 #include <sys/socket.h>
 116 #include <netinet/in.h>
 117
 118 /* Open a GSMTAP source (sending) socket, conncet it to host/port and
 119  * return resulting fd */
 120 int gsmtap_source_init_fd(const char *host, uint16_t port)
 121 {
 122         if (port == 0)
 123                 port = GSMTAP_UDP_PORT;
 124         if (host == NULL)
 125                 host = "localhost";
 126
 127         return osmo_sock_init(AF_UNSPEC, SOCK_DGRAM, IPPROTO_UDP, host, port, 0);
 128 }
 129
 130 int gsmtap_source_add_sink_fd(int gsmtap_fd)
 131 {
 132         struct sockaddr_storage ss;
 133         socklen_t ss_len = sizeof(ss);
 134         int rc;
 135
 136         rc = getpeername(gsmtap_fd, (struct sockaddr *)&ss, &ss_len);
 137         if (rc < 0)
 138                 return rc;
 139
 140         if (osmo_sockaddr_is_local((struct sockaddr *)&ss, ss_len) == 1) {
 141                 rc = osmo_sock_init_sa((struct sockaddr *)&ss, SOCK_DGRAM, IPPROTO_UDP, 1);
 142                 if (rc >= 0)
 143                         return rc;
 144         }
 145
 146         return -ENODEV;
 147 }
 148
 149 int gsmtap_sendmsg(struct gsmtap_inst *gti, struct msgb *msg)
 150 {
 151         if (!gti)
 152                 return -ENODEV;
 153
 154         if (gti->ofd_wq_mode)
 155                 return osmo_wqueue_enqueue(&gti->wq, msg);
 156         else {
 157                 /* try immediate send and return error if any */
 158                 int rc;
 159
 160                 rc = write(gsmtap_inst_fd(gti), msg->data, msg->len);
 161                 if (rc <= 0) {
 162                         return rc;
 163                 } else if (rc >= msg->len) {
 164                         msgb_free(msg);
 165                         return 0;
 166                 } else {
 167                         /* short write */
 168                         return -EIO;
 169                 }
 170         }
 171 }
 172
 173 /* receive a message from L1/L2 and put it in GSMTAP */
 174 int gsmtap_send(struct gsmtap_inst *gti, uint16_t arfcn, uint8_t ts,
 175                 uint8_t chan_type, uint8_t ss, uint32_t fn,
 176                 int8_t signal_dbm, uint8_t snr, const uint8_t *data,
 177                 unsigned int len)
 178 {
 179         struct msgb *msg;
 180
 181         if (!gti)
 182                 return -ENODEV;
 183
 184         msg = gsmtap_makemsg(arfcn, ts, chan_type, ss, fn, signal_dbm,
 185                              snr, data, len);
 186         if (!msg)
 187                 return -ENOMEM;
 188
 189         return gsmtap_sendmsg(gti, msg);
 190 }
 191
 192 /* Callback from select layer if we can write to the socket */
 193 static int gsmtap_wq_w_cb(struct osmo_fd *ofd, struct msgb *msg)
 194 {
 195         int rc;
 196
 197         rc = write(ofd->fd, msg->data, msg->len);
 198         if (rc < 0) {
 199                 perror("writing msgb to gsmtap fd");
 200                 msgb_free(msg);
 201                 return rc;
 202         }
 203         if (rc != msg->len) {
 204                 perror("short write to gsmtap fd");
 205                 msgb_free(msg);
 206                 return -EIO;
 207         }
 208
 209         msgb_free(msg);
 210         return 0;
 211 }
 212
 213 /* Callback from select layer if we can read from the sink socket */
 214 static int gsmtap_sink_fd_cb(struct osmo_fd *fd, unsigned int flags)
 215 {
 216         int rc;
 217         uint8_t buf[4096];
 218
 219         if (!(flags & BSC_FD_READ))
 220                 return 0;
 221
 222         rc = read(fd->fd, buf, sizeof(buf));
 223         if (rc < 0) {
 224                 perror("reading from gsmtap sink fd");
 225                 return rc;
 226         }
 227         /* simply discard any data arriving on the socket */
 228
 229         return 0;
 230 }
 231
 232 /* Add a local sink to an existing GSMTAP source instance */
 233 int gsmtap_source_add_sink(struct gsmtap_inst *gti)
 234 {
 235         int fd;
 236
 237         fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd(gti));
 238         if (fd < 0)
 239                 return fd;
 240
 241         if (gti->ofd_wq_mode) {
 242                 struct osmo_fd *sink_ofd;
 243
 244                 sink_ofd = &gti->sink_ofd;
 245                 sink_ofd->fd = fd;
 246                 sink_ofd->when = BSC_FD_READ;
 247                 sink_ofd->cb = gsmtap_sink_fd_cb;
 248
 249                 osmo_fd_register(sink_ofd);
 250         }
 251
 252         return fd;
 253 }
 254
 255 /* like gsmtap_init2() but integrated with libosmocore select.c */
 256 struct gsmtap_inst *gsmtap_source_init(const char *host, uint16_t port,
 257                                         int ofd_wq_mode)
 258 {
 259         struct gsmtap_inst *gti;
 260         int fd;
 261
 262         fd = gsmtap_source_init_fd(host, port);
 263         if (fd < 0)
 264                 return NULL;
 265
 266         gti = talloc_zero(NULL, struct gsmtap_inst);
 267         gti->ofd_wq_mode = ofd_wq_mode;
 268         gti->wq.bfd.fd = fd;
 269         gti->sink_ofd.fd = -1;
 270
 271         if (ofd_wq_mode) {
 272                 osmo_wqueue_init(&gti->wq, 64);
 273                 gti->wq.write_cb = &gsmtap_wq_w_cb;
 274
 275                 osmo_fd_register(&gti->wq.bfd);
 276         }
 277
 278         return gti;
 279 }
 280
 281 #endif /* HAVE_SYS_SOCKET_H */