int up_addrenv_create(size_t textsize, size_t datasize, size_t heapsize,
                      FAR group_addrenv_t *addrenv)
{
  FAR struct z180_cbr_s *cbr;
  irqstate_t flags;
  size_t envsize;
  uintptr_t alloc;
  unsigned int npages;
  int ret;

  /* Convert the size from bytes to numbers of pages */

#ifdef CONFIG_BUILD_KERNEL
  envsize = textsize + datasize + heapsize;
#else
  envsize = textsize + datasize;
#endif

  npages  = PHYS_ALIGNUP(envsize);
  if (npages < 1)
    {
      /* No address environment... but I suppose that is not an error */

      sdbg("ERROR: npages is zero\n");
      return OK;
    }

  /* Allocate a structure in the common .bss to hold information about the
   * task's address environment.  NOTE that this is not a part of the TCB,
   * but rather a break-away structure that can be shared by the task as
   * well as other threads.  That is necessary because the life of the
   * address of environment might be longer than the life of the task.
   */

  flags = irqsave();
  cbr = z180_mmu_alloccbr();
  if (!cbr)
    {
      sdbg("ERROR: No free CBR structures\n");
      ret = -ENOMEM;
      goto errout_with_irq;
    }

  /* Now allocate the physical memory to back up the address environment */

#ifdef CONFIG_GRAN_SINGLE
  alloc = (uintptr_t)gran_alloc(npages);
#else
  alloc = (uintptr_t)gran_alloc(g_physhandle, npages);
#endif
  if (!alloc)
    {
      sdbg("ERROR: Failed to allocate %d pages\n", npages);
      ret = -ENOMEM;
      goto errout_with_cbr;
    }

  /* Save the information in the CBR structure.  Note that alloc is in
   * 4KB pages, already in the right form for the CBR.
   */

  DEBUGASSERT(alloc <= 0xff);

  cbr->cbr     = (uint8_t)alloc;
  cbr->pages   = (uint8_t)npages;
  *addrenv     = (group_addrenv_t)cbr;

  irqrestore(flags);
  return OK;

errout_with_cbr:
  z180_mmu_freecbr(cbr);

errout_with_irq:
  irqrestore(flags);
  return ret;
}
int schedule_unload(pid_t pid, FAR struct binary_s *bin)
{
  struct sigaction act;
  struct sigaction oact;
  sigset_t sigset;
  irqstate_t flags;
  int errorcode;
  int ret;

  /* Make sure that SIGCHLD is unmasked */

  (void)sigemptyset(&sigset);
  (void)sigaddset(&sigset, SIGCHLD);
  ret = sigprocmask(SIG_UNBLOCK, &sigset, NULL);
  if (ret != OK)
    {
      /* The errno value will get trashed by the following debug output */

      errorcode = get_errno();
      bvdbg("ERROR: sigprocmask failed: %d\n", ret);
      goto errout;
    }

  /* Add the structure to the list.  We want to do this *before* connecting
   * the signal handler.  This does, however, make error recovery more
   * complex if sigaction() fails below because then we have to remove the
   * unload structure for the list in an unexpected context.
   */

  unload_list_add(pid, bin);

  /* Register the SIGCHLD handler */

  act.sa_sigaction = unload_callback;
  act.sa_flags     = SA_SIGINFO;

  (void)sigfillset(&act.sa_mask);
  (void)sigdelset(&act.sa_mask, SIGCHLD);

  ret = sigaction(SIGCHLD, &act, &oact);
  if (ret != OK)
    {
      /* The errno value will get trashed by the following debug output */

      errorcode = get_errno();
      bvdbg("ERROR: sigaction failed: %d\n" , ret);

      /* Emergency removal from the list */

      flags = irqsave();
      if (unload_list_remove(pid) != bin)
        {
          blldbg("ERROR: Failed to remove structure\n");
        }

      irqrestore(flags);
      goto errout;
    }

  return OK;

errout:
  set_errno(errorcode);
  return ERROR;
}
Example #3
0
int stm32_configgpio(uint32_t cfgset)
{
  uint32_t base;
  uint32_t cr;
  uint32_t regval;
  uint32_t regaddr;
  unsigned int port;
  unsigned int pin;
  unsigned int pos;
  unsigned int modecnf;
  irqstate_t flags;
  bool input;

  /* Verify that this hardware supports the select GPIO port */

  port = (cfgset & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT;
  if (port >= STM32_NGPIO_PORTS)
    {
      return -EINVAL;
    }

  /* Get the port base address */

  base = g_gpiobase[port];

  /* Get the pin number and select the port configuration register for that
   * pin
   */

  pin = (cfgset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT;
  if (pin < 8)
    {
      cr  = base + STM32_GPIO_CRL_OFFSET;
      pos = pin;
    }
  else
    {
      cr  = base + STM32_GPIO_CRH_OFFSET;
      pos = pin - 8;
    }

  /* Input or output? */

  input = ((cfgset & GPIO_INPUT) != 0);

  /* Interrupts must be disabled from here on out so that we have mutually
   * exclusive access to all of the GPIO configuration registers.
   */

  flags = irqsave();

  /* Decode the mode and configuration */

  regval  = getreg32(cr);

  if (input)
    {
      /* Input.. force mode = INPUT */

      modecnf = 0;
    }
  else
    {
      /* Output or alternate function */

      modecnf = (cfgset & GPIO_MODE_MASK) >> GPIO_MODE_SHIFT;
    }

  modecnf |= ((cfgset & GPIO_CNF_MASK) >> GPIO_CNF_SHIFT) << 2;

  /* Set the port configuration register */

  regval &= ~(GPIO_CR_MODECNF_MASK(pos));
  regval |= (modecnf << GPIO_CR_MODECNF_SHIFT(pos));
  putreg32(regval, cr);

  /* Set or reset the corresponding BRR/BSRR bit */

  if (!input)
    {
      /* It is an output or an alternate function.  We have to look at the CNF
       * bits to know which.
       */

      unsigned int cnf = (cfgset & GPIO_CNF_MASK);
      if (cnf != GPIO_CNF_OUTPP && cnf != GPIO_CNF_OUTOD)
        {
          /* Its an alternate function pin... we can return early */

          irqrestore(flags);
          return OK;
        }
    }
  else
    {
      /* It is an input pin... Should it configured as an EXTI interrupt? */

      if ((cfgset & GPIO_EXTI) != 0)
        {
          int shift;

          /* Yes.. Set the bits in the EXTI CR register */

          regaddr = STM32_AFIO_EXTICR(pin);
          regval  = getreg32(regaddr);
          shift   = AFIO_EXTICR_EXTI_SHIFT(pin);
          regval &= ~(AFIO_EXTICR_PORT_MASK << shift);
          regval |= (((uint32_t)port) << shift);

          putreg32(regval, regaddr);
        }

      if ((cfgset & GPIO_CNF_MASK) != GPIO_CNF_INPULLUD)
        {
          /* Neither... we can return early */

          irqrestore(flags);
          return OK;
        }
    }

  /* If it is an output... set the pin to the correct initial state.
   * If it is pull-down or pull up, then we need to set the ODR
   * appropriately for that function.
   */

  if ((cfgset & GPIO_OUTPUT_SET) != 0)
    {
      /* Use the BSRR register to set the output */

      regaddr = base + STM32_GPIO_BSRR_OFFSET;
    }
  else
    {
      /* Use the BRR register to clear */

      regaddr = base + STM32_GPIO_BRR_OFFSET;
    }

  regval  = getreg32(regaddr);
  regval |= (1 << pin);
  putreg32(regval, regaddr);

  irqrestore(flags);
  return OK;
}
Example #4
0
static void usbhost_hub_event(FAR void *arg)
{
  FAR struct usbhost_class_s *hubclass;
  FAR struct usbhost_hubport_s *hport;
  FAR struct usbhost_hubport_s *connport;
  FAR struct usbhost_hubpriv_s *priv;
  FAR struct usb_ctrlreq_s *ctrlreq;
  struct usb_portstatus_s portstatus;
  irqstate_t flags;
  uint16_t status;
  uint16_t change;
  uint16_t mask;
  uint16_t feat;
  uint8_t statuschange;
  int port;
  int ret;

  DEBUGASSERT(arg != NULL);
  hubclass = (FAR struct usbhost_class_s *)arg;
  priv     = &((FAR struct usbhost_hubclass_s *)hubclass)->hubpriv;

  /* Has the hub been disconnected? */

  if (priv->disconnected)
    {
      uvdbg("Disconnected\n");
      return;
    }

  /* No.. then set up to process the hub event */

  DEBUGASSERT(priv->ctrlreq);
  ctrlreq = priv->ctrlreq;

  DEBUGASSERT(hubclass->hport);
  hport = hubclass->hport;

  statuschange = priv->buffer[0];
  uvdbg("StatusChange: %02x\n", statuschange);

  /* Check for status change on any port */

  for (port = 1; port <= priv->nports; port++)
    {
      /* Check if port status has changed */

      if ((statuschange & (1 << port)) == 0)
        {
          continue;
        }

      uvdbg("Port %d status change\n", port);

      /* Port status changed, check what happened */

      statuschange &= ~(1 << port);

      /* Read hub port status */

      ctrlreq->type = USB_REQ_DIR_IN | USBHUB_REQ_TYPE_PORT;
      ctrlreq->req  = USBHUB_REQ_GETSTATUS;
      usbhost_putle16(ctrlreq->value, 0);
      usbhost_putle16(ctrlreq->index, port);
      usbhost_putle16(ctrlreq->len, USB_SIZEOF_PORTSTS);

      ret = DRVR_CTRLIN(hport->drvr, hport->ep0, ctrlreq,
                        (FAR uint8_t *)&portstatus);
      if (ret < 0)
        {
          udbg("ERROR: Failed to read port %d status: %d\n", port, ret);
          continue;
        }

      status = usbhost_getle16(portstatus.status);
      change = usbhost_getle16(portstatus.change);

      /* First, clear all change bits */

      mask = 1;
      feat = USBHUB_PORT_FEAT_CCONNECTION;
      while (change)
        {
          if (change & mask)
            {
              ctrlreq->type = USBHUB_REQ_TYPE_PORT;
              ctrlreq->req  = USBHUB_REQ_CLEARFEATURE;
              usbhost_putle16(ctrlreq->value, feat);
              usbhost_putle16(ctrlreq->index, port);
              usbhost_putle16(ctrlreq->len, 0);

              ret = DRVR_CTRLOUT(hport->drvr, hport->ep0, ctrlreq, NULL);
              if (ret < 0)
                {
                  udbg("ERROR: Failed to clear port %d change mask %04x: %d\n",
                       port, mask, ret);
                }

              change &= (~mask);
            }

          mask <<= 1;
          feat++;
        }

      change = usbhost_getle16(portstatus.change);

      /* Handle connect or disconnect, no power management */

      if ((change & USBHUB_PORT_STAT_CCONNECTION) != 0)
        {
          uint16_t debouncetime = 0;
          uint16_t debouncestable = 0;
          uint16_t connection = 0xffff;

          uvdbg("Port %d status %04x change %04x\n", port, status, change);

          /* Debounce */

          while (debouncetime < 1500)
            {
              ctrlreq->type = USB_REQ_DIR_IN | USBHUB_REQ_TYPE_PORT;
              ctrlreq->req  = USBHUB_REQ_GETSTATUS;
              usbhost_putle16(ctrlreq->value, 0);
              usbhost_putle16(ctrlreq->index, port);
              usbhost_putle16(ctrlreq->len, USB_SIZEOF_PORTSTS);

              ret = DRVR_CTRLIN(hport->drvr, hport->ep0, ctrlreq,
                                (FAR uint8_t *)&portstatus);
              if (ret < 0)
                {
                  udbg("ERROR: Failed to get port %d status: %d\n", port, ret);
                  break;
                }

              status = usbhost_getle16(portstatus.status);
              change = usbhost_getle16(portstatus.change);

              if ((change & USBHUB_PORT_STAT_CCONNECTION) == 0 &&
                  (status & USBHUB_PORT_STAT_CONNECTION)  == connection)
                {
                  debouncestable += 25;
                  if (debouncestable >= 100)
                    {
                      uvdbg("Port %d debouncestable=%d\n", port, debouncestable);
                      break;
                    }
                }
              else
                {
                  debouncestable = 0;
                  connection = status & USBHUB_PORT_STAT_CONNECTION;
                }

              if ((change & USBHUB_PORT_STAT_CCONNECTION) != 0)
                {
                  ctrlreq->type = USBHUB_REQ_TYPE_PORT;
                  ctrlreq->req  = USBHUB_REQ_CLEARFEATURE;
                  usbhost_putle16(ctrlreq->value, USBHUB_PORT_FEAT_CCONNECTION);
                  usbhost_putle16(ctrlreq->index, port);
                  usbhost_putle16(ctrlreq->len, 0);

                  (void)DRVR_CTRLOUT(hport->drvr, hport->ep0, ctrlreq, NULL);
                }

              debouncetime += 25;
              usleep(25*1000);
            }

          if (ret < 0 || debouncetime >= 1500)
            {
              udbg("ERROR: Failed to debounce port %d: %d\n", port, ret);
              continue;
            }

          if ((status & USBHUB_PORT_STAT_CONNECTION) != 0)
            {
              /* Device connected to a port on the hub */

              uvdbg("Connection on port %d\n", port);

              ctrlreq->type = USBHUB_REQ_TYPE_PORT;
              ctrlreq->req  = USBHUB_REQ_SETFEATURE;
              usbhost_putle16(ctrlreq->value, USBHUB_PORT_FEAT_RESET);
              usbhost_putle16(ctrlreq->index, port);
              usbhost_putle16(ctrlreq->len, 0);

              ret = DRVR_CTRLOUT(hport->drvr, hport->ep0, ctrlreq, NULL);
              if (ret < 0)
                {
                  udbg("ERROR: Failed to reset port %d: %d\n", port, ret);
                  continue;
                }

              usleep(100*1000);

              ctrlreq->type = USB_REQ_DIR_IN | USBHUB_REQ_TYPE_PORT;
              ctrlreq->req  = USBHUB_REQ_GETSTATUS;
              usbhost_putle16(ctrlreq->value, 0);
              usbhost_putle16(ctrlreq->index, port);
              usbhost_putle16(ctrlreq->len, USB_SIZEOF_PORTSTS);

              ret = DRVR_CTRLIN(hport->drvr, hport->ep0, ctrlreq,
                                (FAR uint8_t *)&portstatus);
              if (ret < 0)
                {
                  udbg("ERROR: Failed to get port %d status: %d\n", port, ret);
                  continue;
                }

              status = usbhost_getle16(portstatus.status);
              change = usbhost_getle16(portstatus.change);

              uvdbg("port %d status %04x change %04x after reset\n",
                    port, status, change);

              if ((status & USBHUB_PORT_STAT_RESET)  == 0 &&
                  (status & USBHUB_PORT_STAT_ENABLE) != 0)
                {
                  if ((change & USBHUB_PORT_STAT_CRESET) != 0)
                    {
                      ctrlreq->type = USBHUB_REQ_TYPE_PORT;
                      ctrlreq->req  = USBHUB_REQ_CLEARFEATURE;
                      usbhost_putle16(ctrlreq->value, USBHUB_PORT_FEAT_CRESET);
                      usbhost_putle16(ctrlreq->index, port);
                      usbhost_putle16(ctrlreq->len, 0);

                      (void)DRVR_CTRLOUT(hport->drvr, hport->ep0, ctrlreq, NULL);
                    }

                  connport = &priv->hport[port];
                  if ((status & USBHUB_PORT_STAT_HIGH_SPEED) != 0)
                    {
                      connport->speed = USB_SPEED_HIGH;
                    }
                  else if ((status & USBHUB_PORT_STAT_LOW_SPEED) != 0)
                    {
                      connport->speed = USB_SPEED_LOW;
                    }
                  else
                    {
                      connport->speed = USB_SPEED_FULL;
                    }

                  /* Activate the hub port by assigning it a control endpoint. */

                  ret = usbhost_hport_activate(connport);
                  if (ret < 0)
                    {
                      udbg("ERROR: usbhost_hport_activate failed: %d\n", ret);
                    }
                  else
                    {
                      /* Inform waiters that a new device has been connected */

                      ret = DRVR_CONNECT(connport->drvr, connport, true);
                      if (ret < 0)
                        {
                          udbg("ERROR: DRVR_CONNECT failed: %d\n", ret);
                          usbhost_hport_deactivate(connport);
                        }
                    }
                }
              else
                {
                  udbg("ERROR: Failed to enable port %d\n", port);
                  continue;
                }
            }
          else
            {
              /* Device disconnected from a port on the hub.  Release port
               * resources.
               */

              uvdbg("Disconnection on port %d\n", port);

              /* Free any devices classes connect on this hub port */

              connport = &priv->hport[port];
              if (connport->devclass != NULL)
                {
                  CLASS_DISCONNECTED(connport->devclass);
                  connport->devclass = NULL;
                }

              /* Free any resources used by the hub port */

              usbhost_hport_deactivate(connport);
            }
        }
      else if (change)
        {
          udbg("WARNING: status %04x change %04x not handled\n", status, change);
        }
    }

  /* Check for hub status change */

  if ((statuschange & 1) != 0)
    {
      /* Hub status changed */

      udbg("WARNING: Hub status changed, not handled\n");
    }

  /* The preceding sequence of events may take a significant amount of
   * time and it is possible that the hub may have been removed while this
   * logic operated.  In any event, we will get here after several failures.
   * But we do not want to schedule another hub event if the hub has been
   * removed.
   */

  flags = irqsave();
  if (!priv->disconnected)
    {
      /* Wait for the next hub event */

      ret = DRVR_ASYNCH(hport->drvr, priv->intin, (FAR uint8_t *)priv->buffer,
                        INTIN_BUFSIZE, usbhost_callback, hubclass);
      if (ret < 0)
        {
          udbg("ERROR: Failed to queue interrupt endpoint: %d\n", ret);
        }
    }

  irqrestore(flags);
}
Example #5
0
int
Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_conversion_interval);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_conversion_interval)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = irqsave();

			if (!_reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}

			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	case AIRSPEEDIOCSSCALE: {
			struct airspeed_scale *s = (struct airspeed_scale *)arg;
			_diff_pres_offset = s->offset_pa;
			return OK;
		}

	case AIRSPEEDIOCGSCALE: {
			struct airspeed_scale *s = (struct airspeed_scale *)arg;
			s->offset_pa = _diff_pres_offset;
			s->scale = 1.0f;
			return OK;
		}

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
int
IIS328DQ::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

				/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_call_interval = 0;
				return OK;

				/* external signalling not supported */
			case SENSOR_POLLRATE_EXTERNAL:

				/* zero would be bad */
			case 0:
				return -EINVAL;

				/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT:
				return ioctl(filp, SENSORIOCSPOLLRATE, IIS328DQ_DEFAULT_RATE);

				/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_call_interval == 0);

					/* convert hz to hrt interval via microseconds */
					unsigned ticks = 1000000 / arg;

					/* check against maximum sane rate */
					if (ticks < 1000)
						return -EINVAL;

					/* update interval for next measurement */
					/* XXX this is a bit shady, but no other way to adjust... */
					_call_interval = ticks;

					/* adjust filters */
					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
					float sample_rate = 1.0e6f/ticks;
					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_call_interval == 0)
			return SENSOR_POLLRATE_MANUAL;

		return 1000000 / _call_interval;

	case SENSORIOCSQUEUEDEPTH: {
		/* lower bound is mandatory, upper bound is a sanity check */
		if ((arg < 1) || (arg > 100))
			return -EINVAL;

		irqstate_t flags = irqsave();
		if (!ringbuf_resize(_reports, arg)) {
			irqrestore(flags);
			return -ENOMEM;
		}
		irqrestore(flags);

		return OK;
	}

	case SENSORIOCGQUEUEDEPTH:
		return ringbuf_size(_reports);

	case SENSORIOCRESET:
		reset();
		return OK;

	case ACCELIOCSSAMPLERATE:
		return set_samplerate(arg, _accel_onchip_filter_bandwidth);

	case ACCELIOCGSAMPLERATE:
		return _accel_samplerate;

	case ACCELIOCSLOWPASS: {
		// set the software lowpass cut-off in Hz
		float cutoff_freq_hz = arg;
		float sample_rate = 1.0e6f / _call_interval;
		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);

		return OK;
	}

	case ACCELIOCGLOWPASS:
		return static_cast<int>(_accel_filter_x.get_cutoff_freq());

	case ACCELIOCSSCALE:
		/* copy scale in */
		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
		return OK;

	case ACCELIOCGSCALE:
		/* copy scale out */
		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
		return OK;

	case ACCELIOCSRANGE:
		/* arg should be in dps accel	*/
		return set_range(arg);

	case ACCELIOCGRANGE:
		/* convert to m/s^2 and return rounded in G */
		return (unsigned long)((_accel_range_m_s2)/IIS328DQ_ONE_G + 0.5f);

	case ACCELIOCSELFTEST:
		return self_test();

	case ACCELIOCSHWLOWPASS:
		return set_samplerate(_accel_samplerate, arg);

	case ACCELIOCGHWLOWPASS:
		return _accel_onchip_filter_bandwidth;//set_samplerate函数中赋值

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}
Example #7
0
static void work_process(FAR struct wqueue_s *wqueue)
{
  volatile FAR struct work_s *work;
  worker_t  worker;
  irqstate_t flags;
  FAR void *arg;
  uint32_t elapsed;
  uint32_t remaining;
  uint32_t next;

  /* Then process queued work.  We need to keep interrupts disabled while
   * we process items in the work list.
   */

  next  = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
  flags = irqsave();
  work  = (FAR struct work_s *)wqueue->q.head;
  while (work)
    {
      /* Is this work ready?  It is ready if there is no delay or if
       * the delay has elapsed. qtime is the time that the work was added
       * to the work queue.  It will always be greater than or equal to
       * zero.  Therefore a delay of zero will always execute immediately.
       */

      elapsed = clock_systimer() - work->qtime;
      if (elapsed >= work->delay)
        {
          /* Remove the ready-to-execute work from the list */

          (void)dq_rem((struct dq_entry_s *)work, &wqueue->q);

          /* Extract the work description from the entry (in case the work
           * instance by the re-used after it has been de-queued).
           */

          worker = work->worker;
          arg    = work->arg;

          /* Mark the work as no longer being queued */

          work->worker = NULL;

          /* Do the work.  Re-enable interrupts while the work is being
           * performed... we don't have any idea how long that will take!
           */

          irqrestore(flags);
          worker(arg);

          /* Now, unfortunately, since we re-enabled interrupts we don't
           * know the state of the work list and we will have to start
           * back at the head of the list.
           */

          flags = irqsave();
          work  = (FAR struct work_s *)wqueue->q.head;
        }
      else
        {
          /* This one is not ready.. will it be ready before the next
           * scheduled wakeup interval?
           */

          /* Here: elapsed < work->delay */
          remaining = work->delay - elapsed;
          if (remaining < next)
            {
              /* Yes.. Then schedule to wake up when the work is ready */

              next = remaining;
            }
              
          /* Then try the next in the list. */

          work = (FAR struct work_s *)work->dq.flink;
        }
    }

  /* Wait awhile to check the work list.  We will wait here until either
   * the time elapses or until we are awakened by a signal.
   */

  usleep(next * USEC_PER_TICK);
  irqrestore(flags);
}
void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
{
  /* Refuse to handle nested signal actions */

  sdbg("tcb=0x%p sigdeliver=0x%p\n", tcb, sigdeliver);

  if (!tcb->xcp.sigdeliver)
    {
      irqstate_t flags;

      /* Make sure that interrupts are disabled */

      flags = irqsave();

      /* First, handle some special cases when the signal is being delivered
       * to the currently executing task.
       */

      sdbg("rtcb=0x%p current_regs=0x%p\n", g_readytorun.head, current_regs);

      if (tcb == (struct tcb_s*)g_readytorun.head)
        {
          /* CASE 1:  We are not in an interrupt handler and a task is
           * signalling itself for some reason.
           */

          if (!current_regs)
            {
              /* In this case just deliver the signal now. */

              sigdeliver(tcb);
            }

          /* CASE 2:  We are in an interrupt handler AND the interrupted
           * task is the same as the one that must receive the signal, then
           * we will have to modify the return state as well as the state
           * in the TCB.
           *
           * Hmmm... there looks like a latent bug here: The following logic
           * would fail in the strange case where we are in an interrupt
           * handler, the thread is signalling itself, but a context switch
           * to another task has occurred so that current_regs does not
           * refer to the thread at g_readytorun.head!
           */

          else
            {
              /* Save the return lr and cpsr and one scratch register
               * These will be restored by the signal trampoline after
               * the signals have been delivered.
               */

              tcb->xcp.sigdeliver       = sigdeliver;
              tcb->xcp.saved_pc         = current_regs[REG_PC];
              tcb->xcp.saved_cpsr       = current_regs[REG_CPSR];

              /* Then set up to vector to the trampoline with interrupts
               * disabled
               */

              current_regs[REG_PC]      = (uint32_t)up_sigdeliver;
              current_regs[REG_CPSR]    = (PSR_MODE_SVC | PSR_I_BIT | PSR_F_BIT);
              if (current_regs[REG_PC] & 0x1)
                {
                  current_regs[REG_CPSR] |= PSR_T_BIT;
                }

              /* And make sure that the saved context in the TCB is the same
               * as the interrupt return context.
               */

              up_savestate(tcb->xcp.regs);
            }
        }

      /* Otherwise, we are (1) signaling a task is not running from an
       * interrupt handler or (2) we are not in an interrupt handler and the
       * running task is signalling some non-running task.
       */

      else
        {
          /* Save the return lr and cpsr and one scratch register.  These
           * will be restored by the signal trampoline after the signals
           * have been delivered.
           */

          tcb->xcp.sigdeliver       = sigdeliver;
          tcb->xcp.saved_pc         = tcb->xcp.regs[REG_PC];
          tcb->xcp.saved_cpsr       = tcb->xcp.regs[REG_CPSR];

          /* Then set up to vector to the trampoline with interrupts
           * disabled
           */

          tcb->xcp.regs[REG_PC]      = (uint32_t)up_sigdeliver;
          tcb->xcp.regs[REG_CPSR]    = (PSR_MODE_SVC | PSR_I_BIT | PSR_F_BIT);
          if (tcb->xcp.regs[REG_PC] & 0x1)
            {
              tcb->xcp.regs[REG_CPSR] |= PSR_T_BIT;
            }
        }

      irqrestore(flags);
    }
}
Example #9
0
static void stm32_idlepm(void)
{
#ifdef CONFIG_RTC_ALARM
  struct timespec alarmtime;
#endif
  static enum pm_state_e oldstate = PM_NORMAL;
  enum pm_state_e newstate;
  irqstate_t flags;
  int ret;

  /* Decide, which power saving level can be obtained */

  newstate = pm_checkstate();

  /* Check for state changes */

  if (newstate != oldstate)
    {
      lldbg("newstate= %d oldstate=%d\n", newstate, oldstate);

      flags = irqsave();

      /* Force the global state change */

      ret = pm_changestate(newstate);
      if (ret < 0)
        {
          /* The new state change failed, revert to the preceding state */

          (void)pm_changestate(oldstate);

          /* No state change... */

          goto errout;
        }

      /* Then perform board-specific, state-dependent logic here */

      switch (newstate)
        {
        case PM_NORMAL:
          {
          }
          break;

        case PM_IDLE:
          {
          }
          break;

        case PM_STANDBY:
          {
#ifdef CONFIG_RTC_ALARM
            /* Disable RTC Alarm interrupt */

#warning "missing logic"

            /* Configure the RTC alarm to Auto Wake the system */

#warning "missing logic"

            /* The tv_nsec value must not exceed 1,000,000,000. That
             * would be an invalid time.
             */

#warning "missing logic"

            /* Set the alarm */

#warning "missing logic"
#endif
            /* Call the STM32 stop mode */

            stm32_pmstop(true);

            /* We have been re-awakened by some even:  A button press?
             * An alarm?  Cancel any pending alarm and resume the normal
             * operation.
             */

#ifdef CONFIG_RTC_ALARM
#warning "missing logic"
#endif
            /* Resume normal operation */

            pm_changestate(PM_NORMAL);
            newstate = PM_NORMAL;
          }
          break;

        case PM_SLEEP:
          {
            /* We should not return from standby mode.  The only way out
             * of standby is via the reset path.
             */

            (void)stm32_pmstandby();
          }
          break;

        default:
          break;
        }

      /* Save the new state */

      oldstate = newstate;

errout:
      irqrestore(flags);
    }
}
void arm_addrenv_destroy_region(FAR uintptr_t **list, unsigned int listlen,
                                uintptr_t vaddr)
{
  irqstate_t flags;
  uintptr_t paddr;
  FAR uint32_t *l2table;
#ifndef CONFIG_ARCH_PGPOOL_MAPPING
  uint32_t l1save;
#endif
  int i;
  int j;

  bvdbg("listlen=%d vaddr=%08lx\n", listlen, (unsigned long)vaddr);

  for (i = 0; i < listlen; vaddr += SECTION_SIZE, list++, i++)
    {
      /* Unhook the L2 page table from the L1 page table */

      mmu_l1_clrentry(vaddr);

      /* Has this page table been allocated? */

      paddr = (uintptr_t)list[i];
      if (paddr != 0)
        {
          flags = irqsave();

#ifdef CONFIG_ARCH_PGPOOL_MAPPING
          /* Get the virtual address corresponding to the physical page address */

          l2table = (FAR uint32_t *)arm_pgvaddr(paddr);
#else
          /* Temporarily map the page into the virtual address space */

          l1save = mmu_l1_getentry(ARCH_SCRATCH_VBASE);
          mmu_l1_setentry(paddr & ~SECTION_MASK, ARCH_SCRATCH_VBASE, MMU_MEMFLAGS);
          l2table = (FAR uint32_t *)(ARCH_SCRATCH_VBASE | (paddr & SECTION_MASK));
#endif

          /* Return the allocated pages to the page allocator */

          for (j = 0; j < ENTRIES_PER_L2TABLE; j++)
            {
              paddr = *l2table++;
              if (paddr != 0)
                {
                  paddr &= PTE_SMALL_PADDR_MASK;
                  mm_pgfree(paddr, 1);
                }
            }

#ifndef CONFIG_ARCH_PGPOOL_MAPPING
          /* Restore the scratch section L1 page table entry */

          mmu_l1_restore(ARCH_SCRATCH_VBASE, l1save);
#endif
          irqrestore(flags);

          /* And free the L2 page table itself */

          mm_pgfree((uintptr_t)list[i], 1);
        }
    }
}
Example #11
0
int sched_setpriority(FAR struct tcb_s *tcb, int sched_priority)
{
  FAR struct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head;
  tstate_t task_state;
  irqstate_t saved_state;

  /* Verify that the requested priority is in the valid range */

  if (sched_priority < SCHED_PRIORITY_MIN || 
      sched_priority > SCHED_PRIORITY_MAX)
    {
      errno = EINVAL;
      return ERROR;
    }

  /* We need to assure that there there is no interrupt activity while
   * performing the following.
   */

  saved_state = irqsave();

  /* There are four cases that must be considered: */

  task_state = tcb->task_state;
  switch (task_state)
    {
      /* CASE 1. The task is running or ready-to-run and a context switch
       * may be caused by the re-prioritization 
       */

      case TSTATE_TASK_RUNNING:

        /* A context switch will occur if the new priority of the running
         * task becomes less than OR EQUAL TO the next highest priority
         * ready to run task.
         */

        if (sched_priority <= tcb->flink->sched_priority)
          {
            /* A context switch will occur. */

            up_reprioritize_rtr(tcb, (uint8_t)sched_priority);
          }

        /* Otherwise, we can just change priority since it has no effect */

        else
          {
            /* Change the task priority */

            tcb->sched_priority = (uint8_t)sched_priority;
          }
        break;

      /* CASE 2. The task is running or ready-to-run and a context switch
       * may be caused by the re-prioritization
       */

      case TSTATE_TASK_READYTORUN:

        /* A context switch will occur if the new priority of the ready-to
         * run task is (strictly) greater than the current running task 
         */

        if (sched_priority > rtcb->sched_priority)
          {
            /* A context switch will occur. */

            up_reprioritize_rtr(tcb, (uint8_t)sched_priority);
          }

        /* Otherwise, we can just change priority and re-schedule (since it
         * have no other effect).
         */

        else
          {
            /* Remove the TCB from the ready-to-run task list */

            ASSERT(!sched_removereadytorun(tcb));

            /* Change the task priority */

            tcb->sched_priority = (uint8_t)sched_priority;

            /* Put it back into the ready-to-run task list */

            ASSERT(!sched_addreadytorun(tcb));
          }
        break;

      /* CASE 3. The task is not in the ready to run list.  Changing its
       * Priority cannot effect the currently executing task.
       */

      default:

        /* CASE 3a. The task resides in a prioritized list. */

        if (g_tasklisttable[task_state].prioritized)
          {
            /* Remove the TCB from the prioritized task list */

            dq_rem((FAR dq_entry_t*)tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list);

            /* Change the task priority */

            tcb->sched_priority = (uint8_t)sched_priority;

            /* Put it back into the prioritized list at the correct
             * position
             */

            sched_addprioritized(tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list);
          }

        /* CASE 3b. The task resides in a non-prioritized list. */

        else
          {
            /* Just change the task's priority */

            tcb->sched_priority = (uint8_t)sched_priority;
          }
        break;
    }

  irqrestore(saved_state);
  return OK;
}
int arm_addrenv_create_region(FAR uintptr_t **list, unsigned int listlen,
                              uintptr_t vaddr, size_t regionsize,
                              uint32_t mmuflags)
{
  irqstate_t flags;
  uintptr_t paddr;
  FAR uint32_t *l2table;
#ifndef CONFIG_ARCH_PGPOOL_MAPPING
  uint32_t l1save;
#endif
  size_t nmapped;
  unsigned int npages;
  unsigned int i;
  unsigned int j;

  bvdbg("listlen=%d vaddr=%08lx regionsize=%ld, mmuflags=%08x\n",
        listlen, (unsigned long)vaddr, (unsigned long)regionsize,
        (unsigned int)mmuflags);

  /* Verify that we are configured with enough virtual address space to
   * support this memory region.
   *
   *   npages pages correspondes to (npages << MM_PGSHIFT) bytes
   *   listlen sections corresponds to (listlen << 20) bytes
   */

  npages = MM_NPAGES(regionsize);
  if (npages > (listlen << (20 - MM_PGSHIFT)))
    {
      bdbg("ERROR: npages=%u listlen=%u\n", npages, listlen);
      return -E2BIG;
    }

  /* Back the allocation up with physical pages and set up the level mapping
   * (which of course does nothing until the L2 page table is hooked into
   * the L1 page table).
   */

  nmapped = 0;
  for (i = 0; i < npages; i += ENTRIES_PER_L2TABLE)
    {
      /* Allocate one physical page for the L2 page table */

      paddr = mm_pgalloc(1);
      if (!paddr)
        {
          return -ENOMEM;
        }

      DEBUGASSERT(MM_ISALIGNED(paddr));
      list[i] = (FAR uintptr_t *)paddr;

      flags = irqsave();

#ifdef CONFIG_ARCH_PGPOOL_MAPPING
      /* Get the virtual address corresponding to the physical page address */

      l2table = (FAR uint32_t *)arm_pgvaddr(paddr);
#else
      /* Temporarily map the page into the virtual address space */

      l1save = mmu_l1_getentry(ARCH_SCRATCH_VBASE);
      mmu_l1_setentry(paddr & ~SECTION_MASK, ARCH_SCRATCH_VBASE, MMU_MEMFLAGS);
      l2table = (FAR uint32_t *)(ARCH_SCRATCH_VBASE | (paddr & SECTION_MASK));
#endif

      /* Initialize the page table */

      memset(l2table, 0, ENTRIES_PER_L2TABLE * sizeof(uint32_t));

      /* Back up L2 entries with physical memory */

      for (j = 0; j < ENTRIES_PER_L2TABLE && nmapped < regionsize; j++)
        {
          /* Allocate one physical page for region data */

          paddr = mm_pgalloc(1);
          if (!paddr)
            {
#ifndef CONFIG_ARCH_PGPOOL_MAPPING
              mmu_l1_restore(ARCH_SCRATCH_VBASE, l1save);
#endif
              irqrestore(flags);
              return -ENOMEM;
            }

          /* Map the .text region virtual address to this physical address */

          set_l2_entry(l2table, paddr, vaddr, mmuflags);
          nmapped += MM_PGSIZE;
          vaddr   += MM_PGSIZE;
        }

      /* Make sure that the initialized L2 table is flushed to physical
       * memory.
       */

      arch_flush_dcache((uintptr_t)l2table,
                        (uintptr_t)l2table +
                        ENTRIES_PER_L2TABLE * sizeof(uint32_t));

#ifndef CONFIG_ARCH_PGPOOL_MAPPING
      /* Restore the scratch section L1 page table entry */

      mmu_l1_restore(ARCH_SCRATCH_VBASE, l1save);
#endif
      irqrestore(flags);
    }

  return npages;
}
Example #13
0
int sem_timedwait(FAR sem_t *sem, FAR const struct timespec *abstime)
{
  FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
  irqstate_t flags;
  int        ticks;
  int        errcode;
  int        ret = ERROR;

  DEBUGASSERT(up_interrupt_context() == false && rtcb->waitdog == NULL);

  /* Verify the input parameters and, in case of an error, set
   * errno appropriately.
   */

#ifdef CONFIG_DEBUG
  if (!abstime || !sem)
    {
      errcode = EINVAL;
      goto errout;
    }
#endif

  /* Create a watchdog.  We will not actually need this watchdog
   * unless the semaphore is unavailable, but we will reserve it up
   * front before we enter the following critical section.
   */

  rtcb->waitdog = wd_create();
  if (!rtcb->waitdog)
    {
      errcode = ENOMEM;
      goto errout;
    }

  /* We will disable interrupts until we have completed the semaphore
   * wait.  We need to do this (as opposed to just disabling pre-emption)
   * because there could be interrupt handlers that are asynchronously
   * posting semaphores and to prevent race conditions with watchdog
   * timeout.  This is not too bad because interrupts will be re-
   * enabled while we are blocked waiting for the semaphore.
   */

  flags = irqsave();

  /* Try to take the semaphore without waiting. */

  ret = sem_trywait(sem);
  if (ret == OK)
    {
      /* We got it! */

      irqrestore(flags);
      wd_delete(rtcb->waitdog);
      rtcb->waitdog = NULL;
      return OK;
    }

  /* We will have to wait for the semaphore.  Make sure that we were provided
   * with a valid timeout.
   */

  if (abstime->tv_nsec < 0 || abstime->tv_nsec >= 1000000000)
    {
      errcode = EINVAL;
      goto errout_disabled;
    }

  /* Convert the timespec to clock ticks.  We must have interrupts
   * disabled here so that this time stays valid until the wait begins.
   */

  errcode = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);

  /* If the time has already expired return immediately. */

  if (errcode == OK && ticks <= 0)
    {
      errcode = ETIMEDOUT;
      goto errout_disabled;
    }

  /* Handle any time-related errors */

  if (errcode != OK)
    {
      goto errout_disabled;
    }

  /* Start the watchdog */

  errcode = OK;
  wd_start(rtcb->waitdog, ticks, (wdentry_t)sem_timeout, 1, getpid());

  /* Now perform the blocking wait */

  ret = sem_wait(sem);
  if (ret < 0)
    {
      /* sem_wait() failed.  Save the errno value */

      errcode = get_errno();
    }

  /* Stop the watchdog timer */

  wd_cancel(rtcb->waitdog);

  /* We can now restore interrupts and delete the watchdog */

  irqrestore(flags);
  wd_delete(rtcb->waitdog);
  rtcb->waitdog = NULL;

  /* We are either returning success or an error detected by sem_wait()
   * or the timeout detected by sem_timeout().
   */

  if (ret < 0)
    {
      /* On failure, restore the errno value returned by sem_wait */

      set_errno(errcode);
    }

  return ret;

/* Error exits */

errout_disabled:
  irqrestore(flags);
  wd_delete(rtcb->waitdog);
  rtcb->waitdog = NULL;

errout:
  set_errno(errcode);
  return ERROR;
}
bool
uORB::DeviceNode::appears_updated(SubscriberData *sd)
{
	/* assume it doesn't look updated */
	bool ret = false;

	/* avoid racing between interrupt and non-interrupt context calls */
	irqstate_t state = irqsave();

	/* check if this topic has been published yet, if not bail out */
	if (_data == nullptr) {
		ret = false;
		goto out;
	}

	/*
	 * If the subscriber's generation count matches the update generation
	 * count, there has been no update from their perspective; if they
	 * don't match then we might have a visible update.
	 */
	while (sd->generation != _generation) {

		/*
		 * Handle non-rate-limited subscribers.
		 */
		if (sd->update_interval == 0) {
			ret = true;
			break;
		}

		/*
		 * If we have previously told the subscriber that there is data,
		 * and they have not yet collected it, continue to tell them
		 * that there has been an update.  This mimics the non-rate-limited
		 * behaviour where checking / polling continues to report an update
		 * until the topic is read.
		 */
		if (sd->update_reported) {
			ret = true;
			break;
		}

		/*
		 * If the interval timer is still running, the topic should not
		 * appear updated, even though at this point we know that it has.
		 * We have previously been through here, so the subscriber
		 * must have collected the update we reported, otherwise
		 * update_reported would still be true.
		 */
		if (!hrt_called(&sd->update_call)) {
			break;
		}

		/*
		 * Make sure that we don't consider the topic to be updated again
		 * until the interval has passed once more by restarting the interval
		 * timer and thereby re-scheduling a poll notification at that time.
		 */
		hrt_call_after(&sd->update_call,
			       sd->update_interval,
			       &uORB::DeviceNode::update_deferred_trampoline,
			       (void *)this);

		/*
		 * Remember that we have told the subscriber that there is data.
		 */
		sd->update_reported = true;
		ret = true;

		break;
	}

out:
	irqrestore(state);

	/* consider it updated */
	return ret;
}
Example #15
0
int
LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
		switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_call_accel_interval = 0;
				return OK;

			/* external signalling not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);

			case SENSOR_POLLRATE_DEFAULT:
				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);

				/* adjust to a legal polling interval in Hz */
			default: {
				/* do we need to start internal polling? */
				bool want_start = (_call_accel_interval == 0);

				/* convert hz to hrt interval via microseconds */
				unsigned ticks = 1000000 / arg;

				/* check against maximum sane rate */
				if (ticks < 500)
					return -EINVAL;

				/* adjust filters */
				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());

				/* update interval for next measurement */
				/* XXX this is a bit shady, but no other way to adjust... */
				_accel_call.period = _call_accel_interval = ticks;

				/* if we need to start the poll state machine, do it */
				if (want_start)
					start();

				return OK;
			}
		}
	}

	case SENSORIOCGPOLLRATE:
		if (_call_accel_interval == 0)
			return SENSOR_POLLRATE_MANUAL;

		return 1000000 / _call_accel_interval;

	case SENSORIOCSQUEUEDEPTH: {
		/* lower bound is mandatory, upper bound is a sanity check */
		if ((arg < 1) || (arg > 100))
			return -EINVAL;

		irqstate_t flags = irqsave();
		if (!_accel_reports->resize(arg)) {
			irqrestore(flags);
			return -ENOMEM;
		}
		irqrestore(flags);

		return OK;
	}

	case SENSORIOCGQUEUEDEPTH:
		return _accel_reports->size();

	case SENSORIOCRESET:
		reset();
		return OK;

	case ACCELIOCSSAMPLERATE:
		return accel_set_samplerate(arg);

	case ACCELIOCGSAMPLERATE:
		return _accel_samplerate;

	case ACCELIOCSLOWPASS: {
		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
	}

	case ACCELIOCGLOWPASS:
		return _accel_filter_x.get_cutoff_freq();

	case ACCELIOCSSCALE: {
		/* copy scale, but only if off by a few percent */
		struct accel_scale *s = (struct accel_scale *) arg;
		float sum = s->x_scale + s->y_scale + s->z_scale;
		if (sum > 2.0f && sum < 4.0f) {
			memcpy(&_accel_scale, s, sizeof(_accel_scale));
			return OK;
		} else {
			return -EINVAL;
		}
	}

	case ACCELIOCSRANGE:
		/* arg needs to be in G */
		return accel_set_range(arg);

	case ACCELIOCGRANGE:
		/* convert to m/s^2 and return rounded in G */
		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);

	case ACCELIOCGSCALE:
		/* copy scale out */
		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
		return OK;

	case ACCELIOCSELFTEST:
		return accel_self_test();

	default:
		/* give it to the superclass */
		return SPI::ioctl(filp, cmd, arg);
	}
}
Example #16
0
int mq_send(mqd_t mqdes, const void *msg, size_t msglen, int prio)
{
  FAR struct mqueue_inode_s  *msgq;
  FAR struct mqueue_msg_s *mqmsg = NULL;
  irqstate_t saved_state;
  int ret = ERROR;

  /* Verify the input parameters -- setting errno appropriately
   * on any failures to verify.
   */

  if (mq_verifysend(mqdes, msg, msglen, prio) != OK)
    {
      return ERROR;
    }

  /* Get a pointer to the message queue */

  sched_lock();
  msgq = mqdes->msgq;

  /* Allocate a message structure:
   * - Immediately if we are called from an interrupt handler.
   * - Immediately if the message queue is not full, or
   * - After successfully waiting for the message queue to become
   *   non-FULL.  This would fail with EAGAIN, EINTR, or ETIMEOUT.
   */

  saved_state = irqsave();
  if (up_interrupt_context()      || /* In an interrupt handler */
      msgq->nmsgs < msgq->maxmsgs || /* OR Message queue not full */
      mq_waitsend(mqdes) == OK)      /* OR Successfully waited for mq not full */
    {
      /* Allocate the message */

      irqrestore(saved_state);
      mqmsg = mq_msgalloc();
    }
  else
    {
      /* We cannot send the message (and didn't even try to allocate it)
       * because:
       * - We are not in an interrupt handler AND
       * - The message queue is full AND
       * - When we tried waiting, the wait was unsuccessful.
       */

      irqrestore(saved_state);
    }

  /* Check if we were able to get a message structure -- this can fail
   * either because we cannot send the message (and didn't bother trying
   * to allocate it) or because the allocation failed.
   */

  if (mqmsg)
    {
      /* Yes, perform the message send. */

      ret = mq_dosend(mqdes, mqmsg, msg, msglen, prio);
    }

  sched_unlock();
  return ret;
}
Example #17
0
void platform_idle(void)
{
    irqflags_t flags = di();
    tty_poll();
    irqrestore(flags);
}
Example #18
0
int mq_timedsend(mqd_t mqdes, const char *msg, size_t msglen, int prio,
                 const struct timespec *abstime)
{
  WDOG_ID      wdog;
  FAR msgq_t  *msgq;
  FAR mqmsg_t *mqmsg = NULL;
  irqstate_t   saved_state;
  int          ret = ERROR;

  DEBUGASSERT(up_interrupt_context() == false);

  /* Verify the input parameters -- setting errno appropriately
   * on any failures to verify.
   */

  if (mq_verifysend(mqdes, msg, msglen, prio) != OK)
    {
      return ERROR;
    }

  if (!abstime || abstime->tv_sec < 0 || abstime->tv_nsec > 1000000000)
    {
      set_errno(EINVAL);
      return ERROR;
    }

  /* Get a pointer to the message queue */

  msgq = mqdes->msgq;

  /* Create a watchdog.  We will not actually need this watchdog
   * unless the queue is full, but we will reserve it up front
   * before we enter the following critical section.
   */

  wdog = wd_create();
  if (!wdog)
    {
      set_errno(EINVAL);
      return ERROR;
    }

  /* Allocate a message structure:
   * - If we are called from an interrupt handler, or
   * - If the message queue is not full, or
   */

  sched_lock();
  saved_state = irqsave();
  if (up_interrupt_context()      || /* In an interrupt handler */
      msgq->nmsgs < msgq->maxmsgs)   /* OR Message queue not full */
    {
      /* Allocate the message */

      irqrestore(saved_state);
      mqmsg = mq_msgalloc();
    }
  else
    {
      int ticks;

      /* We are not in an interupt handler and the message queue is full.
       * set up a timed wait for the message queue to become non-full.
       *
       * Convert the timespec to clock ticks.  We must have interrupts
       * disabled here so that this time stays valid until the wait begins.
       */

      int result = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);

      /* If the time has already expired and the message queue is empty,
       * return immediately.
       */

      if (result == OK && ticks <= 0)
        {
          result = ETIMEDOUT;
        }

      /* Handle any time-related errors */

      if (result != OK)
        {
          set_errno(result);
          ret = ERROR;
        }

      /* Start the watchdog and begin the wait for MQ not full */

      if (result == OK)
        {
          /* Start the watchdog */

          wd_start(wdog, ticks, (wdentry_t)mq_sndtimeout, 1, getpid());

          /* And wait for the message queue to be non-empty */

          ret = mq_waitsend(mqdes);

          /* This may return with an error and errno set to either EINTR
           * or ETIMEOUT.  Cancel the watchdog timer in any event.
           */

          wd_cancel(wdog);
        }

      /* That is the end of the atomic operations */

      irqrestore(saved_state);

      /* If any of the above failed, set the errno.  Otherwise, there should
       * be space for another message in the message queue.  NOW we can allocate
       * the message structure.
       */

      if (ret == OK)
        {
          mqmsg = mq_msgalloc();
        }
    }

  /* Check if we were able to get a message structure -- this can fail
   * either because we cannot send the message (and didn't bother trying
   * to allocate it) or because the allocation failed.
   */

  if (mqmsg)
    {
      /* Yes, peform the message send. */

      ret = mq_dosend(mqdes, mqmsg, msg, msglen, prio);
    }

  sched_unlock();
  wd_delete(wdog);
  return ret;
}
uintptr_t pgalloc(uintptr_t brkaddr, unsigned int npages)
{
  FAR struct tcb_s *tcb = sched_self();
  FAR struct task_group_s *group;
  FAR uint32_t *l2table;
  irqstate_t flags;
  uintptr_t paddr;
#ifndef CONFIG_ARCH_PGPOOL_MAPPING
  uint32_t l1save;
#endif
  unsigned int index;

  DEBUGASSERT(tcb && tcb->group);
  group = tcb->group;

  /* The current implementation only supports extending the user heap
   * region as part of the implementation of user sbrk().  This function
   * needs to be expanded to also handle (1) extending the user stack
   * space and (2) extending the kernel memory regions as well.
   */

  DEBUGASSERT((group->tg_flags & GROUP_FLAG_ADDRENV) != 0);

  /* brkaddr = 0 means that no heap has yet been allocated */

  if (brkaddr == 0)
    {
      brkaddr = CONFIG_ARCH_HEAP_VBASE;
    }

  DEBUGASSERT(brkaddr >= CONFIG_ARCH_HEAP_VBASE && brkaddr < ARCH_HEAP_VEND);
  DEBUGASSERT(MM_ISALIGNED(brkaddr));

  for (; npages > 0; npages--)
    {
      /* Get the physical address of the level 2 page table */

      paddr = get_pgtable(&group->tg_addrenv, brkaddr);
      if (paddr == 0)
        {
          return 0;
        }

      flags = irqsave();

#ifdef CONFIG_ARCH_PGPOOL_MAPPING
      /* Get the virtual address corresponding to the physical page address */

      l2table = (FAR uint32_t *)arm_pgvaddr(paddr);
#else
      /* Temporarily map the level 2 page table into the "scratch" virtual
       * address space
       */

      l1save = mmu_l1_getentry(ARCH_SCRATCH_VBASE);
      mmu_l1_setentry(paddr & ~SECTION_MASK, ARCH_SCRATCH_VBASE, MMU_MEMFLAGS);
      l2table = (FAR uint32_t *)(ARCH_SCRATCH_VBASE | (paddr & SECTION_MASK));
#endif

      /* Back up L2 entry with physical memory */

      paddr = mm_pgalloc(1);
      if (paddr == 0)
        {
#ifndef CONFIG_ARCH_PGPOOL_MAPPING
          mmu_l1_restore(ARCH_SCRATCH_VBASE, l1save);
#endif
          irqrestore(flags);
          return 0;
        }

      /* The table divides a 1Mb address space up into 256 entries, each
       * corresponding to 4Kb of address space.  The page table index is
       * related to the offset from the beginning of 1Mb region.
       */

      index = (brkaddr & 0x000ff000) >> 12;

       /* Map the .text region virtual address to this physical address */

      DEBUGASSERT(l2table[index] == 0);
      l2table[index] = paddr | MMU_L2_UDATAFLAGS;
      brkaddr += MM_PGSIZE;

      /* Make sure that the modified L2 table is flushed to physical
       * memory.
       */

      arch_flush_dcache((uintptr_t)&l2table[index],
                        (uintptr_t)&l2table[index] + sizeof(uint32_t));

#ifndef CONFIG_ARCH_PGPOOL_MAPPING
      /* Restore the scratch L1 page table entry */

      mmu_l1_restore(ARCH_SCRATCH_VBASE, l1save);
#endif
      irqrestore(flags);
    }

  return brkaddr;
}
Example #20
0
int sem_tickwait(FAR sem_t *sem, systime_t start, uint32_t delay)
{
  FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
  irqstate_t flags;
  systime_t elapsed;
  int ret;

  DEBUGASSERT(sem != NULL && up_interrupt_context() == false &&
              rtcb->waitdog == NULL);

  /* Create a watchdog.  We will not actually need this watchdog
   * unless the semaphore is unavailable, but we will reserve it up
   * front before we enter the following critical section.
   */

  rtcb->waitdog = wd_create();
  if (!rtcb->waitdog)
    {
      return -ENOMEM;
    }

  /* We will disable interrupts until we have completed the semaphore
   * wait.  We need to do this (as opposed to just disabling pre-emption)
   * because there could be interrupt handlers that are asynchronously
   * posting semaphores and to prevent race conditions with watchdog
   * timeout.  This is not too bad because interrupts will be re-
   * enabled while we are blocked waiting for the semaphore.
   */

  flags = irqsave();

  /* Try to take the semaphore without waiting. */

  ret = sem_trywait(sem);
  if (ret == OK)
    {
      /* We got it! */

      goto success_with_irqdisabled;
    }

  /* We will have to wait for the semaphore.  Make sure that we were provided
   * with a valid timeout.
   */

  if (delay == 0)
    {
      /* Return the errno from sem_trywait() */

      ret = -get_errno();
      goto errout_with_irqdisabled;
    }

  /* Adjust the delay for any time since the delay was calculated */

  elapsed = clock_systimer() - start;
  if (/* elapsed >= (UINT32_MAX / 2) || */ elapsed >= delay)
    {
      ret = -ETIMEDOUT;
      goto errout_with_irqdisabled;
    }

  delay -= elapsed;

  /* Start the watchdog with interrupts still disabled */

  (void)wd_start(rtcb->waitdog, delay, (wdentry_t)sem_timeout, 1, getpid());

  /* Now perform the blocking wait */

  ret = sem_wait(sem);
  if (ret < 0)
    {
      /* Return the errno from sem_wait() */

      ret = -get_errno();
      goto errout_with_irqdisabled;
    }

  /* Stop the watchdog timer */

  wd_cancel(rtcb->waitdog);

  /* We can now restore interrupts and delete the watchdog */

  /* Success exits */

success_with_irqdisabled:

  /* Error exits */

errout_with_irqdisabled:
  irqrestore(flags);
  wd_delete(rtcb->waitdog);
  rtcb->waitdog = NULL;
  return ret;
}
Example #21
0
static void usbhost_disconnect_event(FAR void *arg)
{
  FAR struct usbhost_class_s *hubclass = (FAR struct usbhost_class_s *)arg;
  FAR struct usbhost_hubpriv_s *priv;
  FAR struct usbhost_hubport_s *hport;
  FAR struct usbhost_hubport_s *child;
  irqstate_t flags;
  int port;

  uvdbg("Disconnecting\n");

  DEBUGASSERT(hubclass != NULL && hubclass->hport != NULL);
  priv  = &((FAR struct usbhost_hubclass_s *)hubclass)->hubpriv;
  hport = hubclass->hport;

  uvdbg("Destroying hub on port  %d\n", hport->port);

  /* Set an indication to any users of the device that the device is no
   * longer available.
   */

  flags = irqsave();

  /* Cancel any pending transfers on the interrupt IN pipe */

  DRVR_CANCEL(hport->drvr, priv->intin);

  /* Cancel any pending port status change events */

  work_cancel(LPWORK, &priv->work);

  /* Disable power to all downstream ports */

  (void)usbhost_hubpwr(priv, hport, false);

  /* Free the allocated control request */

  DRVR_FREE(hport->drvr, (FAR uint8_t *)priv->ctrlreq);

  /* Free buffer for status change (INT) endpoint */

  DRVR_IOFREE(hport->drvr, priv->buffer);

  /* Destroy the interrupt IN endpoint */

  DRVR_EPFREE(hport->drvr, priv->intin);

  /* Release per-port resources */

  for (port = 0; port < USBHUB_MAX_PORTS; port++)
    {
      /* Free any devices classes connect on this hub port */

      child = &priv->hport[port];
      if (child->devclass != NULL)
        {
          CLASS_DISCONNECTED(child->devclass);
          child->devclass = NULL;
        }

      /* Free any resources used by the hub port */

      usbhost_hport_deactivate(child);
    }

  /* Deactivate the parent hub port (unless it is the root hub port) */

  usbhost_hport_deactivate(hport);

  /* Destroy the semaphores */

  sem_destroy(&priv->exclsem);

  /* Disconnect the USB host device */

  DRVR_DISCONNECT(hport->drvr, hport);

  /* Free the class instance */

  kmm_free(hubclass);
  hport->devclass = NULL;
  irqrestore(flags);
}
Example #22
0
int group_initialize(FAR struct task_tcb_s *tcb)
{
	FAR struct task_group_s *group;
#if defined(HAVE_GROUP_MEMBERS) || defined(CONFIG_ARCH_ADDRENV)
	irqstate_t flags;
#endif

	DEBUGASSERT(tcb && tcb->cmn.group);
	group = tcb->cmn.group;

#ifdef HAVE_GROUP_MEMBERS
	/* Allocate space to hold GROUP_INITIAL_MEMBERS members of the group */

	group->tg_members = (FAR pid_t *)kmm_malloc(GROUP_INITIAL_MEMBERS * sizeof(pid_t));
	if (!group->tg_members) {
		kmm_free(group);
		return -ENOMEM;
	}

	/* Assign the PID of this new task as a member of the group. */

	group->tg_members[0] = tcb->cmn.pid;

	/* Initialize the non-zero elements of group structure and assign it to
	 * the tcb.
	 */

	group->tg_mxmembers = GROUP_INITIAL_MEMBERS;	/* Number of members in allocation */

#endif

#if defined(HAVE_GROUP_MEMBERS) || defined(CONFIG_ARCH_ADDRENV)
	/* Add the initialized entry to the list of groups */

	flags = irqsave();
	group->flink = g_grouphead;
	g_grouphead = group;
	irqrestore(flags);

#endif

	/* Save the ID of the main task within the group of threads.  This needed
	 * for things like SIGCHILD.  It ID is also saved in the TCB of the main
	 * task but is also retained in the group which may persist after the main
	 * task has exited.
	 */

#if !defined(CONFIG_DISABLE_PTHREAD) && defined(CONFIG_SCHED_HAVE_PARENT)
	group->tg_task = tcb->cmn.pid;
#endif
#if defined(CONFIG_SCHED_ATEXIT) && !defined(CONFIG_SCHED_ONEXIT)
	/* atexit support *********************************************************** */

	sq_init(&(group->tg_atexitfunc));
#endif

#ifdef CONFIG_SCHED_ONEXIT
	/* on_exit support ********************************************************** */

	sq_init(&(group->tg_onexitfunc));
#endif

	/* Mark that there is one member in the group, the main task */

	group->tg_nmembers = 1;
	return OK;
}
Example #23
0
static void up_idlepm(void)
{
  static enum pm_state_e oldstate = PM_NORMAL;
  enum pm_state_e newstate;
  irqstate_t flags;
  int ret;

  /* Decide, which power saving level can be obtained */

  newstate = pm_checkstate();

  /* Check for state changes */

  if (newstate != oldstate)
    {
      flags = irqsave();

      /* Perform board-specific, state-dependent logic here */

      llvdbg("newstate= %d oldstate=%d\n", newstate, oldstate);

      /* Then force the global state change */

      ret = pm_changestate(newstate);
      if (ret < 0)
        {
          /* The new state change failed, revert to the preceding state */

          (void)pm_changestate(oldstate);
        }
      else
        {
          /* Save the new state */

          oldstate = newstate;
        }

      /* MCU-specific power management logic */

      switch (newstate)
        {
        case PM_NORMAL:
          break;

        case PM_IDLE:
          break;

        case PM_STANDBY:
          sam_pmstop(true);
          break;

        case PM_SLEEP:
          (void)sam_pmstandby();
          break;

        default:
          break;
        }

      irqrestore(flags);
    }
}
Example #24
0
uint8_t chksigs(void)
{
	uint8_t j;
	uint32_t pending = udata.u_ptab->p_pending & ~udata.u_ptab->p_held;
	int (**svec)(int) = &udata.u_sigvec[0];
	uint32_t m;

	/* Fast path - no signals pending means no work.
	   Cursig being set means we've already worked out what to do.
	 */

rescan:
	if (udata.u_cursig || !pending || udata.u_ptab->p_status == P_STOPPED)
		return udata.u_cursig;

	/* Dispatch the lowest numbered signal */
	for (j = 1; j < NSIGS; ++j) {
		svec++;
		m = sigmask(j);
		if (!(m & pending))
			continue;
		/* This is more complex than in V7 - we have multiple
		   behaviours plus core dump */
		if (*svec == SIG_DFL) {
		        /* SIGSTOP can't be ignored and puts the process into
		           P_STOPPED state when it is ready to handle the signal.
		           Annoyingly right now we have to context switch to the task
		           in order to stop it in the right place. That would be nice
		           to fix */
		        if (m & stopper) {
				/* Don't allow us to race SIGCONT */
				irqflags_t irq = di();
				/* FIXME: can we ever end up here not in READY/RUNNING ? */
				nready--;
				udata.u_ptab->p_status = P_STOPPED;
				udata.u_ptab->p_event = j;
				udata.u_ptab->p_pending &= ~m;	// unset the bit
				irqrestore(irq);
				switchout();
				/* Other things may have happened */
				goto rescan;
	                }

			/* The signal is being handled, so clear it even if
			   we are exiting (otherwise we'll loop in
			   chksigs) */
			udata.u_ptab->p_pending &= ~m;

	                if ((m & clear) || udata.u_ptab->p_pid == 1) {
			/* SIGCONT is subtle - we woke the process to handle
			   the signal so ignoring here works fine */
				continue;
			}
#ifdef DEBUG
			kprintf("process terminated by signal %d\n", j);
#endif
			/* We may have marked ourselves as asleep and
			   then been caught by the chksigs when we tried
			   to task switch into bed. In that case we need
			   to put the process back in running state */
			if (udata.u_ptab->p_status == P_SLEEP) {
				udata.u_ptab->p_status = P_RUNNING;
				nready++;
			}
			doexit(dump_core(j));
		} else if (*svec != SIG_IGN) {
			/* Arrange to call the user routine at return */
			udata.u_ptab->p_pending &= ~m;	// unset the bit
#ifdef DEBUG
			kprintf("about to process signal %d\n", j);
#endif
			udata.u_cursig = j;
			break;
		}
	}
	return udata.u_cursig;
}
Example #25
0
static void
comms_handle_command(const void *buffer, size_t length)
{
	const struct px4io_command *cmd = (struct px4io_command *)buffer;

	if (length != sizeof(*cmd))
		return;

	irqstate_t flags = irqsave();

	/* fetch new PWM output values */
	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
		system_state.fmu_channel_data[i] = cmd->output_control[i];

	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
	if (system_state.arm_ok && !cmd->arm_ok)
		system_state.armed = false;

	system_state.arm_ok = cmd->arm_ok;
	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
	system_state.manual_override_ok = cmd->manual_override_ok;
	system_state.mixer_fmu_available = true;
	system_state.fmu_data_received_time = hrt_absolute_time();

	/* set PWM update rate if changed (after limiting) */
	uint16_t new_servo_rate = cmd->servo_rate;

	/* reject faster than 500 Hz updates */
	if (new_servo_rate > 500) {
		new_servo_rate = 500;
	}

	/* reject slower than 50 Hz updates */
	if (new_servo_rate < 50) {
		new_servo_rate = 50;
	}

	if (system_state.servo_rate != new_servo_rate) {
		up_pwm_servo_set_rate(new_servo_rate);
		system_state.servo_rate = new_servo_rate;
	}

	/*
	 * update servo values immediately.
	 * the updates are done in addition also
	 * in the mainloop, since this function will only
	 * update with a connected FMU.
	 */
	mixer_tick();

	/* handle relay state changes here */
	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
		if (system_state.relays[i] != cmd->relay_state[i]) {
			switch (i) {
			case 0:
				POWER_ACC1(cmd->relay_state[i]);
				break;

			case 1:
				POWER_ACC2(cmd->relay_state[i]);
				break;

			case 2:
				POWER_RELAY1(cmd->relay_state[i]);
				break;

			case 3:
				POWER_RELAY2(cmd->relay_state[i]);
				break;
			}
		}

		system_state.relays[i] = cmd->relay_state[i];
	}

	irqrestore(flags);
}
Example #26
0
void doexit(uint16_t val)
{
	int16_t j;
	ptptr p;
	irqflags_t irq;

#ifdef DEBUG
	kprintf("process %d exiting %d\n", udata.u_ptab->p_pid, val);

	kprintf
	    ("udata.u_page %u, udata.u_ptab %p, udata.u_ptab->p_page %u\n",
	     udata.u_page, udata.u_ptab, udata.u_ptab->p_page);
#endif
	if (udata.u_ptab->p_pid == 1)
		panic(PANIC_KILLED_INIT);

	sync();		/* Not necessary, but a good idea. */

	irq = di();

	/* We are exiting, hold all signals  (they will never be
	   delivered). If we don't do this we might take a signal
	   while exiting which would be ... unfortunate */
	udata.u_ptab->p_held = 0xFFFFFFFFUL;
	udata.u_cursig = 0;

	/* Discard our memory before we blow away and reuse the memory */
	pagemap_free(udata.u_ptab);

	for (j = 0; j < UFTSIZE; ++j) {
		if (udata.u_files[j] != NO_FILE)
			doclose(j);
	}


	udata.u_ptab->p_exitval = val;

	i_deref(udata.u_cwd);
	i_deref(udata.u_root);

	/* Stash away child's execution tick counts in process table,
	 * overlaying some no longer necessary stuff.
	 *
	 * Pedantically POSIX says we should do this at the point of wait()
	 */
	udata.u_utime += udata.u_cutime;
	udata.u_stime += udata.u_cstime;
	memcpy(&(udata.u_ptab->p_priority), &udata.u_utime,
	       2 * sizeof(clock_t));

	for (p = ptab; p < ptab_end; ++p) {
		if (p->p_status == P_EMPTY || p == udata.u_ptab)
			continue;
		/* Set any child's parents to init */
		if (p->p_pptr == udata.u_ptab) {
			p->p_pptr = ptab;	/* ptab is always init */
			/* Suppose our child is a zombie and init has
			   SIGCLD blocked */
		        if (ptab[0].p_ignored & (1UL << SIGCHLD)) {
				p->p_status = P_EMPTY;
			} else {
				ssig(&ptab[0], SIGCHLD);
				wakeup(&ptab[0]);
			}
		}
		/* Send SIGHUP to any pgrp members and remove
		   them from our pgrp */
                if (p->p_pgrp == udata.u_ptab->p_pid) {
			p->p_pgrp = 0;
			ssig(p, SIGHUP);
			ssig(p, SIGCONT);
		}
	}
	tty_exit();
	irqrestore(irq);
#ifdef DEBUG
	kprintf
	    ("udata.u_page %u, udata.u_ptab %p, udata.u_ptab->p_page %u\n",
	     udata.u_page, udata.u_ptab, udata.u_ptab->p_page);
#endif
#ifdef CONFIG_ACCT
	acctexit(p);
#endif
        udata.u_page = 0xFFFFU;
        udata.u_page2 = 0xFFFFU;
        signal_parent(udata.u_ptab->p_pptr);
	nready--;
	nproc--;

	switchin(getproc());
	panic(PANIC_DOEXIT);
}
Example #27
0
ssize_t mq_timedreceive(mqd_t mqdes, FAR char *msg, size_t msglen,
                        FAR int *prio, FAR const struct timespec *abstime)
{
  FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
  FAR struct mqueue_msg_s *mqmsg;
  irqstate_t saved_state;
  int ret = ERROR;

  DEBUGASSERT(up_interrupt_context() == false && rtcb->waitdog == NULL);

  /* Verify the input parameters and, in case of an error, set
   * errno appropriately.
   */

  if (mq_verifyreceive(mqdes, msg, msglen) != OK)
    {
      return ERROR;
    }

  if (!abstime || abstime->tv_nsec < 0 || abstime->tv_nsec >= 1000000000)
    {
      set_errno(EINVAL);
      return ERROR;
    }

  /* Create a watchdog.  We will not actually need this watchdog
   * unless the queue is not empty, but we will reserve it up front
   * before we enter the following critical section.
   */

  rtcb->waitdog = wd_create();
  if (!rtcb->waitdog)
    {
      set_errno(EINVAL);
      return ERROR;
    }

  /* Get the next message from the message queue.  We will disable
   * pre-emption until we have completed the message received.  This
   * is not too bad because if the receipt takes a long time, it will
   * be because we are blocked waiting for a message and pre-emption
   * will be re-enabled while we are blocked
   */

  sched_lock();

  /* Furthermore, mq_waitreceive() expects to have interrupts disabled
   * because messages can be sent from interrupt level.
   */

  saved_state = irqsave();

  /* Check if the message queue is empty.  If it is NOT empty, then we
   * will not need to start timer.
   */

  if (mqdes->msgq->msglist.head == NULL)
    {
      int ticks;

      /* Convert the timespec to clock ticks.  We must have interrupts
       * disabled here so that this time stays valid until the wait begins.
       */

      int result = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);

      /* If the time has already expired and the message queue is empty,
       * return immediately.
       */

      if (result == OK && ticks <= 0)
        {
          result = ETIMEDOUT;
        }

      /* Handle any time-related errors */

      if (result != OK)
        {
          set_errno(result);
          irqrestore(saved_state);
          sched_unlock();
          wd_delete(rtcb->waitdog);
          rtcb->waitdog = NULL;
          return ERROR;
        }

      /* Start the watchdog */

      wd_start(rtcb->waitdog, ticks, (wdentry_t)mq_rcvtimeout, 1, getpid());
    }

  /* Get the message from the message queue */

  mqmsg = mq_waitreceive(mqdes);

  /* Stop the watchdog timer (this is not harmful in the case where
   * it was never started)
   */

  wd_cancel(rtcb->waitdog);

  /* We can now restore interrupts */

  irqrestore(saved_state);

  /* Check if we got a message from the message queue.  We might
   * not have a message if:
   *
   * - The message queue is empty and O_NONBLOCK is set in the mqdes
   * - The wait was interrupted by a signal
   * - The watchdog timeout expired
   */

  if (mqmsg)
    {
      ret = mq_doreceive(mqdes, mqmsg, msg, prio);
    }

  sched_unlock();
  wd_delete(rtcb->waitdog);
  rtcb->waitdog = NULL;
  return ret;
}
Example #28
0
int
LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
		switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_call_mag_interval = 0;
				return OK;

			/* external signalling not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT:
				/* 100 Hz is max for mag */
				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_call_mag_interval == 0);

					/* convert hz to hrt interval via microseconds */
					unsigned ticks = 1000000 / arg;

					/* check against maximum sane rate */
					if (ticks < 1000)
						return -EINVAL;

					/* update interval for next measurement */
					/* XXX this is a bit shady, but no other way to adjust... */
					_mag_call.period = _call_mag_interval = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_call_mag_interval == 0)
			return SENSOR_POLLRATE_MANUAL;

		return 1000000 / _call_mag_interval;
	
	case SENSORIOCSQUEUEDEPTH: {
		/* lower bound is mandatory, upper bound is a sanity check */
		if ((arg < 1) || (arg > 100))
			return -EINVAL;

		irqstate_t flags = irqsave();
		if (!_mag_reports->resize(arg)) {
			irqrestore(flags);
			return -ENOMEM;
		}
		irqrestore(flags);

		return OK;
	}

	case SENSORIOCGQUEUEDEPTH:
		return _mag_reports->size();

	case SENSORIOCRESET:
		reset();
		return OK;

	case MAGIOCSSAMPLERATE:
		return mag_set_samplerate(arg);

	case MAGIOCGSAMPLERATE:
		return _mag_samplerate;

	case MAGIOCSLOWPASS:
	case MAGIOCGLOWPASS:
		/* not supported, no internal filtering */
		return -EINVAL;

	case MAGIOCSSCALE:
		/* copy scale in */
		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
		return OK;

	case MAGIOCGSCALE:
		/* copy scale out */
		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
		return OK;

	case MAGIOCSRANGE:
		return mag_set_range(arg);

	case MAGIOCGRANGE:
		return _mag_range_ga;

	case MAGIOCSELFTEST:
		return mag_self_test();

	case MAGIOCGEXTERNAL:
		/* no external mag board yet */
		return 0;

	default:
		/* give it to the superclass */
		return SPI::ioctl(filp, cmd, arg);
	}
}
Example #29
0
int
SRF02::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_cycling_rate);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();

					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					int ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_cycling_rate)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = irqsave();

			if (!_reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}

			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	case RANGEFINDERIOCSETMINIUMDISTANCE: {
			set_minimum_distance(*(float *)arg);
			return 0;
		}
		break;

	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
			set_maximum_distance(*(float *)arg);
			return 0;
		}
		break;

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Example #30
0
void up_sigdeliver(void)
{
#ifndef CONFIG_DISABLE_SIGNALS
  struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;
  uint8_t regs[XCPTCONTEXT_SIZE];
  sig_deliver_t sigdeliver;

  /* Save the errno.  This must be preserved throughout the signal handling
   * so that the user code final gets the correct errno value (probably
   * EINTR).
   */

  int saved_errno = rtcb->pterrno;

  board_led_on(LED_SIGNAL);

  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n",
        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);
  ASSERT(rtcb->xcp.sigdeliver != NULL);

  /* Save the real return state on the stack. */

  up_copystate(regs, rtcb->xcp.regs);
  regs[REG_PC]   = rtcb->xcp.saved_pc[0];
  regs[REG_PC+1] = rtcb->xcp.saved_pc[1];
  regs[REG_FLG]  = rtcb->xcp.saved_flg;

  /* Get a local copy of the sigdeliver function pointer.  We do this so
   * that we can nullify the sigdeliver function pointer in the TCB and
   * accept more signal deliveries while processing the current pending
   * signals.
   */

  sigdeliver           = rtcb->xcp.sigdeliver;
  rtcb->xcp.sigdeliver = NULL;

  /* Then restore the task interrupt state. */

  irqrestore(rtcb->xcp.saved_flg);

  /* Deliver the signals */

  sigdeliver(rtcb);

  /* Output any debug messages BEFORE restoring errno (because they may
   * alter errno), then disable interrupts again and restore the original
   * errno that is needed by the user logic (it is probably EINTR).
   */

  sdbg("Resuming\n");
  (void)irqsave();
  rtcb->pterrno = saved_errno;

  /* Then restore the correct state for this thread of
   * execution.
   */

  board_led_off(LED_SIGNAL);
  up_fullcontextrestore(regs);
#endif
}