#define _GNU_SOURCE
#include <sys/types.h>
#include <sys/ioctl.h>
#include <stdarg.h>
#include <signal.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <alloca.h>

struct mq_attr {
	long		mq_flags;	/* O_NONBLOCK or 0 */
	long		mq_maxmsg;	/* Maximum number of messages in the queue */
	long		mq_msgsize;	/* Maximum size of one message in bytes */
	long		mq_curmsgs;	/* Current number of messages in the queue */
	long		__pad[2];
};

struct mq_open {
	char		*mq_name;	/* pathname */
	int		mq_oflag;	/* flags */
	unsigned short	mq_mode;	/* mode */
	struct mq_attr	mq_attr;	/* attributes */
};

struct mq_sndrcv {
	size_t		mq_len;		/* message length */
	long		mq_type;	/* message type */
	char		*mq_buf;	/* message buffer */
};

#define MQ_OPEN		_IOW(0xB2, 0, struct mq_open)
#define MQ_GETATTR	_IOR(0xB2, 1, struct mq_attr)
#define MQ_SEND		_IOW(0xB2, 2, struct mq_sndrcv)
#define MQ_RECEIVE	_IOWR(0xB2, 3, struct mq_sndrcv)
#define MQ_NOTIFY	_IOW(0xB2, 4, struct sigevent)

#define MQ_PRIO_MAX	0x7FFFFFFE

typedef int mqd_t;

/* *********** */
#ifndef __set_errno
#define __set_errno(x) errno = x
#endif
#ifndef __memcpy
#define __memcpy(x,y,z) memcpy(x,y,z)
#endif
#ifndef __mempcpy
#define __mempcpy(x,y,z) mempcpy(x,y,z)
#endif
#ifndef __ioctl
#define __ioctl(x,y,z) ioctl(x,y,z)
#endif
#undef __libc_once
#define __libc_once(x,y) do { } while (0)
/* *********** */

/* Mount point of the shared memory filesystem.  */
static struct
{
  char *dir;
  size_t dirlen;
} mountpoint
= { "/dev/msg/", 9 };

mqd_t mq_open (const char *name, int oflag, ...)
{
  size_t namelen;
  char *fname, *p;
  int fd;
  mqd_t ret;
  mode_t mode = 0;
  struct mq_attr *attr = NULL;
  struct mq_open open_arg;

  /* Determine where the msgfs is mounted.  */
  __libc_once (once, where_is_msgfs);

  /* If we don't know the mount points there is nothing we can do.  Ever.  */
  if (mountpoint.dir == NULL)
    {
      __set_errno (ENOSYS);
      return -1;
    }
      
  /* Construct the filename.  */
  while (name[0] == '/')
    ++name;
                                          
  if (name[0] == '\0')
    {
      /* The name "/" is not supported.  */
      __set_errno (EINVAL);
      return -1;
    }
                                                                      
  namelen = strlen (name);
  fname = (char *) alloca (mountpoint.dirlen + namelen + 1);
  p = __mempcpy (fname, mountpoint.dir, mountpoint.dirlen);

  if (oflag & O_CREAT)
    {
      va_list ap;

      va_start (ap, oflag);
      /* Get the arguments.  */
      mode = va_arg (ap, mode_t);
      attr = va_arg (ap, struct mq_attr *);
      va_end (ap);

      if (attr != NULL)
	{
	  p[0] = '.';
	  p[1] = '\0';
	  fd = open (fname, O_RDONLY);

	  if (fd < 0)
	    {
	      __set_errno (ENOSYS);
	      return -1;
	    }

	  __memcpy (p, name, namelen + 1);
	  open_arg.mq_name = fname;
	  open_arg.mq_oflag = oflag;
	  open_arg.mq_mode = mode;
	  open_arg.mq_attr = *attr;

	  ret = __ioctl (fd, MQ_OPEN, &open_arg);
	  if (ret < 0 && errno == ENOTTY)
	    __set_errno (ENOSYS);

	  close (fd);
	  return ret;
	}
    }

  __memcpy (p, name, namelen + 1);
  return open (fname, oflag, mode);
}

int mq_close (mqd_t mqdes)
{
  return close (mqdes);
}

int mq_send (mqd_t mqdes, const char *buf, size_t len, unsigned int prio)
{
  struct mq_sndrcv send_req;

  if (prio > MQ_PRIO_MAX)
    {
      __set_errno (EINVAL);
      return -1;
    }

  send_req.mq_buf = (char *)buf;
  send_req.mq_len = len;
  send_req.mq_type = MQ_PRIO_MAX - prio;

  return __ioctl (mqdes, MQ_SEND, &send_req);
}

ssize_t mq_receive (mqd_t mqdes, char *buf, size_t len, unsigned int *prio)
{
  struct mq_sndrcv recv_req;
  ssize_t ret;

  recv_req.mq_buf = buf;
  recv_req.mq_len = len;
  recv_req.mq_type = -MQ_PRIO_MAX;

  ret = __ioctl (mqdes, MQ_RECEIVE, &recv_req);

  if (!ret && prio != NULL)
    *prio = MQ_PRIO_MAX - recv_req.mq_type;

  return ret;
}

int mq_unlink (const char *name)
{
  size_t namelen;
  char *fname;

  /* Determine where the msgfs is mounted.  */
  __libc_once (once, where_is_msgfs);

  if (mountpoint.dir == NULL)
    {
      /* We cannot find the shmfs.  If `name' is really a message
         queue object it must have been created by another process
         and we have no idea where that process found the mountpoint.  */
      __set_errno (ENOENT);
      return -1;
    }

  /* Construct the filename.  */
  while (name[0] == '/')
    ++name;

  if (name[0] == '\0')
    {
      /* The name "/" is not supported.  */
      __set_errno (ENOENT);
      return -1;
    }

  namelen = strlen (name);
  fname = (char *) alloca (mountpoint.dirlen + namelen + 1);
  __mempcpy (__mempcpy (fname, mountpoint.dir, mountpoint.dirlen),
             name, namelen + 1);

  /* And get the file descriptor.  */
  return unlink (fname);
}

int mq_getattr (mqd_t mqdes, const struct mq_attr *mqstat)
{
  return __ioctl (mqdes, MQ_GETATTR, mqstat);
}

int mq_setattr (mqd_t mqdes, const struct mq_attr *mqstat,
	        struct mq_attr *omqstat)
{
  int ret;

  if (omqstat != NULL)
    {
      ret = __ioctl (mqdes, MQ_GETATTR, omqstat);
      if (ret)
	return ret;
    }

  ret = fcntl (mqdes, F_GETFL);
  if (ret == -1)
    return ret;

  if ((ret ^ mqstat->mq_flags) & O_NONBLOCK)
    {
      ret = fcntl (mqdes, F_SETFL, (ret & ~O_NONBLOCK)
				   | (mqstat->mq_flags & O_NONBLOCK));
      if (ret == -1)
	return ret;
    }

  return 0;
}

int mq_notify (mqd_t mqdes, const struct sigevent *notification)
{
  struct sigevent null_notify, *n;

  n = (struct sigevent *)notification;
  if (notification == NULL)
    {
      null_notify.sigev_notify = SIGEV_NONE;
      n = &null_notify;
    }
  else if (notification->sigev_notify != SIGEV_NONE
	   && notification->sigev_notify != SIGEV_SIGNAL)
    {
      __set_errno (EINVAL);
      return -1;
    }

  return __ioctl (mqdes, MQ_NOTIFY, notification);
}