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->ofd_wq_mode)
 152                 return osmo_wqueue_enqueue(&gti->wq, msg);
 153         else {
 154                 /* try immediate send and return error if any */
 155                 int rc;
 156
 157                 rc = write(gsmtap_inst_fd(gti), msg->data, msg->len);
 158                 if (rc <= 0) {
 159                         return rc;
 160                 } else if (rc >= msg->len) {
 161                         msgb_free(msg);
 162                         return 0;
 163                 } else {
 164                         /* short write */
 165                         return -EIO;
 166                 }
 167         }
 168 }
 169
 170 /* receive a message from L1/L2 and put it in GSMTAP */
 171 int gsmtap_send(struct gsmtap_inst *gti, uint16_t arfcn, uint8_t ts,
 172                 uint8_t chan_type, uint8_t ss, uint32_t fn,
 173                 int8_t signal_dbm, uint8_t snr, const uint8_t *data,
 174                 unsigned int len)
 175 {
 176         struct msgb *msg;
 177
 178         msg = gsmtap_makemsg(arfcn, ts, chan_type, ss, fn, signal_dbm,
 179                              snr, data, len);
 180         if (!msg)
 181                 return -ENOMEM;
 182
 183         return gsmtap_sendmsg(gti, msg);
 184 }
 185
 186 /* Callback from select layer if we can write to the socket */
 187 static int gsmtap_wq_w_cb(struct osmo_fd *ofd, struct msgb *msg)
 188 {
 189         int rc;
 190
 191         rc = write(ofd->fd, msg->data, msg->len);
 192         if (rc < 0) {
 193                 perror("writing msgb to gsmtap fd");
 194                 msgb_free(msg);
 195                 return rc;
 196         }
 197         if (rc != msg->len) {
 198                 perror("short write to gsmtap fd");
 199                 msgb_free(msg);
 200                 return -EIO;
 201         }
 202
 203         msgb_free(msg);
 204         return 0;
 205 }
 206
 207 /* Callback from select layer if we can read from the sink socket */
 208 static int gsmtap_sink_fd_cb(struct osmo_fd *fd, unsigned int flags)
 209 {
 210         int rc;
 211         uint8_t buf[4096];
 212
 213         if (!(flags & BSC_FD_READ))
 214                 return 0;
 215
 216         rc = read(fd->fd, buf, sizeof(buf));
 217         if (rc < 0) {
 218                 perror("reading from gsmtap sink fd");
 219                 return rc;
 220         }
 221         /* simply discard any data arriving on the socket */
 222
 223         return 0;
 224 }
 225
 226 /* Add a local sink to an existing GSMTAP source instance */
 227 int gsmtap_source_add_sink(struct gsmtap_inst *gti)
 228 {
 229         int fd;
 230
 231         fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd(gti));
 232         if (fd < 0)
 233                 return fd;
 234
 235         if (gti->ofd_wq_mode) {
 236                 struct osmo_fd *sink_ofd;
 237
 238                 sink_ofd = &gti->sink_ofd;
 239                 sink_ofd->fd = fd;
 240                 sink_ofd->when = BSC_FD_READ;
 241                 sink_ofd->cb = gsmtap_sink_fd_cb;
 242
 243                 osmo_fd_register(sink_ofd);
 244         }
 245
 246         return fd;
 247 }
 248
 249 /* like gsmtap_init2() but integrated with libosmocore select.c */
 250 struct gsmtap_inst *gsmtap_source_init(const char *host, uint16_t port,
 251                                         int ofd_wq_mode)
 252 {
 253         struct gsmtap_inst *gti;
 254         int fd;
 255
 256         fd = gsmtap_source_init_fd(host, port);
 257         if (fd < 0)
 258                 return NULL;
 259
 260         gti = talloc_zero(NULL, struct gsmtap_inst);
 261         gti->ofd_wq_mode = ofd_wq_mode;
 262         gti->wq.bfd.fd = fd;
 263         gti->sink_ofd.fd = -1;
 264
 265         if (ofd_wq_mode) {
 266                 osmo_wqueue_init(&gti->wq, 64);
 267                 gti->wq.write_cb = &gsmtap_wq_w_cb;
 268
 269                 osmo_fd_register(&gti->wq.bfd);
 270         }
 271
 272         return gti;
 273 }
 274
 275 #endif /* HAVE_SYS_SOCKET_H */