mapicombo6drv.c 9.75 KB
Newer Older
1 2 3
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
Arne Øslebø's avatar
Arne Øslebø committed
4 5 6 7 8
#include <stdio.h>
#include <stdarg.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
9
#include <stdbool.h>
Arne Øslebø's avatar
Arne Øslebø committed
10 11 12 13
#include <fcntl.h>
#include <time.h>
#include <errno.h>
#include <pthread.h>
14
#include <pcap.h>
Arne Øslebø's avatar
Arne Øslebø committed
15 16 17 18 19 20 21 22 23
#include <sys/mman.h>
#include <sys/shm.h>
#include <sys/time.h>
#include "mapi.h"
#include "mapidrv.h"
#include "mapidlib.h"
#include "mapid.h"
#include "mapidevices.h"
#include "flist.h"
24 25
#include "debug.h"
#include "mapi_errors.h"
's avatar
committed
26

's avatar
committed
27
#include <libsze2.h>
's avatar
committed
28

Arne Øslebø's avatar
Arne Øslebø committed
29
typedef struct combo6_instance {
30 31 32
  int file;
  char *name;
  int id;
Arne Øslebø's avatar
Arne Øslebø committed
33 34
  pthread_attr_t th_attr;
  pthread_t th_proc;
35

Arne Øslebø's avatar
Arne Øslebø committed
36
  int combo6fd;
37 38 39
  struct szedata *sze;

  mapi_offline_device_status_t *offline_status;
Arne Øslebø's avatar
Arne Øslebø committed
40 41 42 43
  mapid_hw_info_t hwinfo;
  mapidlib_instance_t mapidlib;
} combo6_instance_t;

44 45
__attribute__ ((constructor)) void init ();
__attribute__ ((destructor))  void fini ();
Arne Øslebø's avatar
Arne Øslebø committed
46

47
static flist_t *devlist;
Arne Øslebø's avatar
Arne Øslebø committed
48 49 50 51 52 53 54 55 56

/* for mapidlib errorcode */
int
mapidrv_get_errno(int devid, int fd)
{
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_get_errno(&i->mapidlib, fd);
}

57 58 59
#ifdef WITH_AUTHENTICATION
int mapidrv_authenticate(int devid, int fd, char *vo)
{
60 61
  combo6_instance_t *i = flist_get(devlist, devid);
  return mapid_authenticate(&i->mapidlib, fd, vo);
62 63 64
}
#endif

Arne Øslebø's avatar
Arne Øslebø committed
65 66 67 68 69 70 71
int
mapidrv_connect (int devid, int fd)
{
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_connect(&i->mapidlib, fd);
}

's avatar
committed
72
int
73
mapidrv_apply_function (int devid, int fd, int flags, char* function, mapiFunctArg *fargs)
's avatar
committed
74
{
75 76
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_apply_function; devid = %d; fd = %d; function = %s", devid, fd, function));

's avatar
committed
77 78
  combo6_instance_t *i=flist_get(devlist,devid);

79 80 81
  int _flags = flags;

  return mapid_apply_function(&i->mapidlib, fd, function, fargs, _flags);
Arne Øslebø's avatar
Arne Øslebø committed
82 83
}

84
int mapidrv_add_device(const char *devname, int file,int devid, global_function_list_t *gflist,void *olstatus)
Arne Øslebø's avatar
Arne Øslebø committed
85
{
86 87
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_add_device"));

Arne Øslebø's avatar
Arne Øslebø committed
88
  combo6_instance_t *i=malloc(sizeof(combo6_instance_t));
89 90

  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_add_device; devname: %s", devname));
Arne Øslebø's avatar
Arne Øslebø committed
91 92
  i->name=strdup(devname);
  i->id=devid;
93 94 95
  i->file=file;
  i->hwinfo.offline=0;
  i->combo6fd = -1;
96
  i->sze = NULL;
97 98 99 100 101 102 103 104
  i->hwinfo.devfd=i->combo6fd;
  i->hwinfo.gflist=gflist;
  i->hwinfo.pkt_drop=0;

  i->offline_status = olstatus;
  if(devid<0)
    i->hwinfo.offline = 1;

Arne Øslebø's avatar
Arne Øslebø committed
105 106 107 108
#ifdef DEBUG
  printf("Added device %d: %s\n",devid,devname);
#endif
  flist_append(devlist,devid,i);
109 110 111
#ifdef WITH_AUTHENTICATION
  mapid_init(&i->mapidlib, &i->hwinfo);
#else
Arne Øslebø's avatar
Arne Øslebø committed
112
  mapid_init(&i->mapidlib);
113
#endif
Arne Øslebø's avatar
Arne Øslebø committed
114 115 116
  return 0;
}

117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138
int mapidrv_delete_device(int devid)
{
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_delete_device"));

  combo6_instance_t *i=flist_remove(devlist,devid);

  if (i!=NULL) {
    int err=0;

    if (i->th_proc && pthread_equal(i->th_proc, pthread_self())==0) {
      DEBUG_CMD(Debug_Message("Calling thread != th_proc (%lu != %lu), cancelling", i->th_proc, pthread_self()));
      fflush(stdout);

      if ((err=pthread_cancel(i->th_proc))!=0) {
        if (!(i->hwinfo.offline==1 && err==ESRCH)) {
          DEBUG_CMD(Debug_Message("WARNING: Could not cancel thread for devid %d (%s)", devid, strerror(err)));
          fflush(stdout);
        }
      }
    }

    if (i->hwinfo.offline==0) {
's avatar
committed
139 140 141 142
      if(i->sze != NULL) {
        DEBUG_CMD(Debug_Message("szedata_close(%p)", i->sze));
        szedata_close(i->sze);
      }
143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160
    } else {
      if (i->file) {
        close(i->file);
        DEBUG_CMD(Debug_Message("Closed file"));
      }
    }

    mapid_destroy(&i->mapidlib);
    free(i->name);
    if(i->offline_status != NULL)
      *(i->offline_status) = DEVICE_DELETED;

    free(i);
  }

  return 0;
}

Arne Øslebø's avatar
Arne Øslebø committed
161
static void
162
mapidrv_proc_loop (void *arg)
Arne Øslebø's avatar
Arne Øslebø committed
163
{
164 165 166 167 168 169 170 171 172 173 174 175 176
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_proc_loop"));
  int devid = *(int *)arg;
  combo6_instance_t *i=flist_get(devlist,devid);
  int err;
  unsigned int rx = 0xff, tx = 0x00;

  unsigned char *packet;
  unsigned char *dev_pkt;
  unsigned char *link_pkt;
  unsigned int data_len;
  mapid_pkthdr_t mhdr;

  DEBUG_CMD(Debug_Message("combo6drv: subscribing: rx-0x%02hx tx-0x%02hx", rx, tx));
's avatar
committed
177
  err = szedata_subscribe(i->sze, &rx, &tx, 0, 0); //, SZE2_RX_POLL_CNT, SZE2_TX_POLL_CNT);
178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
  if (err) {
    DEBUG_CMD(Debug_Message("combo6drv: ERROR subscring."));
  }
  else {
    DEBUG_CMD(Debug_Message("combo6drv: subscribed: rx-0x%02hx tx-0x%02hx", rx, tx));
  }

  err = szedata_start(i->sze);
  if (err) {
    DEBUG_CMD(Debug_Message("combo6drv: ERROR szedata_start()."));
  }
  else {
    DEBUG_CMD(Debug_Message("combo6drv: szedata_start()."));
  }

  struct timeval ts; // FIXME
Arne Øslebø's avatar
Arne Øslebø committed
194 195 196

  while (1)
    {
197

198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215
      packet = szedata_read_next(i->sze, &data_len);

      //unsigned int data_len;
      unsigned int hw_data_len;
      unsigned char *data;
      unsigned char *hw_data;


      if(packet) {
        //DEBUG_CMD(Debug_Message("combo6drv: packet..."));
        szedata_decode_packet(packet, &data, &hw_data, &data_len, &hw_data_len);
        dev_pkt = hw_data;
        link_pkt = data;
        mhdr.caplen=hw_data_len;
        mhdr.wlen=data_len;

        gettimeofday(&ts, NULL);
        mhdr.ts = (((unsigned long long)ts.tv_sec)<<32)+(((ts.tv_usec << 12) + (ts.tv_usec<<8) - ((ts.tv_usec*1825)>>5)) & 0xffffffff);
Arne Øslebø's avatar
Arne Øslebø committed
216 217 218

        mapid_process_pkt(&i->mapidlib,dev_pkt,link_pkt,&mhdr);
        i->hwinfo.pkts++;
219 220
      }

Arne Øslebø's avatar
Arne Øslebø committed
221 222 223 224
    }
}

int
225 226 227 228 229 230 231 232
mapidrv_read_results (int devid, int fd, int fid, mapid_result_t** result)
{
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_read_results"));
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_read_results(&i->mapidlib,fd,fid,result);
}

mapid_funct_info_t* mapidrv_get_flow_functions(int devid,int fd)
Arne Øslebø's avatar
Arne Øslebø committed
233
{
234 235 236 237 238 239 240 241 242
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_get_flow_functions"));
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_get_flow_functions(&i->mapidlib,fd);
}

int mapidrv_get_flow_info(int devid,int fd,mapi_flow_info_t *info) {
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_get_flow_info"));
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_get_flow_info(&i->mapidlib,fd,info);
Arne Øslebø's avatar
Arne Øslebø committed
243 244 245 246 247
}

int
mapidrv_create_flow (int devid, int fd, char **devtype)
{
248
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_create_flow"));
Arne Øslebø's avatar
Arne Øslebø committed
249
  combo6_instance_t *i=flist_get(devlist,devid);
250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270

  if(devid < 0)
    {
      *devtype=MAPI_DEVICE_SCAMPI;
      i->hwinfo.offline=1;

      i->hwinfo.cap_length=1500;
      i->hwinfo.link_type=DLT_EN10MB;
      i->hwinfo.devtype=MAPI_DEVICE_SCAMPI;
      i->hwinfo.devid=i->id;
      i->hwinfo.pkts=0;

      DEBUG_CMD(Debug_Message("Reading from trace file: %s", i->name));

      return mapid_add_flow(&i->mapidlib,fd,&i->hwinfo,NULL);
    }

  i=flist_get(devlist,devid);

  i->hwinfo.offline=0;

Arne Øslebø's avatar
Arne Øslebø committed
271 272 273
  *devtype=MAPI_DEVICE_SCAMPI;

  //Open device if it is not already open
274
  if (i->sze == NULL)
Arne Øslebø's avatar
Arne Øslebø committed
275
    {
276 277
      if ((i->sze = szedata_open (i->name)) == NULL)   {
        fprintf (stderr, "szedata_open(%s): %s\n", i->name, strerror (errno));
Arne Øslebø's avatar
Arne Øslebø committed
278 279
        return COMBO6_OPEN_ERR;
      }
280
      fprintf (stderr, "szedata_open(%s): %p\n", i->name, i->sze);
Arne Øslebø's avatar
Arne Øslebø committed
281 282 283 284 285 286 287 288 289 290 291 292

      //This should be read from the hardware
      i->hwinfo.link_type=DLT_EN10MB;
      i->hwinfo.cap_length=1500;
      i->hwinfo.devtype=MAPI_DEVICE_SCAMPI;
      i->hwinfo.adapterinfo=&i->combo6fd;
      i->hwinfo.devid=i->id;
      i->hwinfo.pkts=0;

      //Start processing thread
      if (pthread_attr_init (&i->th_attr) != 0)
        {
293
          DEBUG_CMD(Debug_Message("ERROR: pthread_attr_init failed"));
Arne Øslebø's avatar
Arne Øslebø committed
294 295
          return COMBO6_PTHR_ERR;
        }
296
      if (pthread_create(&i->th_proc, &i->th_attr, (void *) mapidrv_proc_loop, (void *) &(i->id)) != 0)
Arne Øslebø's avatar
Arne Øslebø committed
297
        {
298
          DEBUG_CMD(Debug_Message("ERROR: pthread_create failed"));
Arne Øslebø's avatar
Arne Øslebø committed
299 300 301 302
          return COMBO6_PTHR_ERR;
        }
    }

303
  return mapid_add_flow(&i->mapidlib, fd,&i->hwinfo,NULL);
Arne Øslebø's avatar
Arne Øslebø committed
304 305 306 307
}

int mapidrv_load_library(int devid, char* lib)
{
308
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_load_library"));
Arne Øslebø's avatar
Arne Øslebø committed
309 310 311 312 313 314
  return mapid_load_library(lib);
}

int
mapidrv_close_flow (int devid, int fd)
{
315
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_close_flow"));
Arne Øslebø's avatar
Arne Øslebø committed
316 317 318 319
  combo6_instance_t *i=flist_get(devlist,devid);
  return mapid_close_flow (&i->mapidlib, fd);
}

's avatar
committed
320

Arne Øslebø's avatar
Arne Øslebø committed
321 322 323 324 325 326 327 328 329 330 331 332 333 334
__attribute__ ((constructor))
     void init ()
{
  // mapid_init();
  devlist=malloc(sizeof(flist_t));
  flist_init(devlist);
  printf ("Combo6 driver loaded\n");
}

__attribute__ ((destructor))
     void fini ()
{
  printf ("Combo6 driver unloaded\n");
}
335

336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401
void pktdrop(combo6_instance_t *i) {
  char buf[BUFSIZ];
  FILE *pipe;
  unsigned int hfe_x0_clas = 0;
  unsigned int hfe_x1_clas = 0;
  unsigned int crossbar2_trim_unit = 0;

  unsigned int failed = 0;

  if(pipe = popen("csbus 018C0008", "r")) {
    if(fgets(buf, BUFSIZ, pipe) != NULL) {
      if(!sscanf(buf, "%x", &hfe_x0_clas)) failed = 1;
    }
    else failed = 1;
    pclose(pipe);
  }
  else failed = 1;

  if(pipe = popen("csbus 018C000C", "r")) {
    if(fgets(buf, BUFSIZ, pipe) != NULL) {
      if(!sscanf(buf, "%x", &hfe_x1_clas)) failed = 1;;
    }
    else failed = 1;
    pclose(pipe);
  }
  else failed = 1;

  if(pipe = popen("csbus 018C8014", "r")) {
    if(fgets(buf, BUFSIZ, pipe) != NULL) {
      if(!sscanf(buf, "%x", &crossbar2_trim_unit)) failed = 1;
    }
    else failed = 1;
    pclose(pipe);
  }
  else failed = 1;

  if(!failed) {
    i->hwinfo.pkts = hfe_x0_clas + hfe_x1_clas;
    i->hwinfo.pkt_drop = hfe_x0_clas + hfe_x1_clas - crossbar2_trim_unit;
  }
  else {
    DEBUG_CMD(Debug_Message("combo6drv: pktdrop failed"));;
  }
}

int
mapidrv_stats (int devid, char **devtype, struct mapi_stat *stats)
{
  DEBUG_CMD(Debug_Message("combo6drv: mapidrv_stats"));

  combo6_instance_t *i=flist_get(devlist,devid);

  *devtype=MAPI_DEVICE_SCAMPI;

  if (i!=NULL)
  {
  pktdrop(i);
	stats->ps_recv=i->hwinfo.pkts + i->hwinfo.pkt_drop;
	stats->ps_drop=i->hwinfo.pkt_drop;
	stats->ps_ifdrop=0;
	return 0;
  }

  return MAPI_STATS_ERROR;
}

402
/* vim: set shiftwidth=2 tabstop=2 smarttab expandtab : */