drivers/timers/dshot.c - nuttx - Git at Google

 /****************************************************************************
  * drivers/timers/dshot.c
  *
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed to the Apache Software Foundation (ASF) under one or more
  * contributor license agreements.  See the NOTICE file distributed with
  * this work for additional information regarding copyright ownership.  The
  * ASF licenses this file to you under the Apache License, Version 2.0 (the
  * "License"); you may not use this file except in compliance with the
  * License.  You may obtain a copy of the License at
  *
  *   http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
  * License for the specific language governing permissions and limitations
  * under the License.
  *
  ****************************************************************************/

 /****************************************************************************
  * Included Files
  ****************************************************************************/

 #include <nuttx/config.h>

 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <stdlib.h>
 #include <string.h>
 #include <errno.h>
 #include <debug.h>

 #include <nuttx/fs/fs.h>
 #include <nuttx/kmalloc.h>
 #include <nuttx/mutex.h>
 #include <nuttx/timers/dshot.h>

 #ifdef CONFIG_DSHOT

 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/

 #define DSHOT_GCR_MASK       0x000fffffu
 #define DSHOT_ERPMSCALE_NUM  600000u

 /****************************************************************************
  * Private Types
  ****************************************************************************/

 struct dshot_upperhalf_s
 {
   uint8_t crefs;
   mutex_t lock;
   FAR struct dshot_lowerhalf_s *dev;
   struct dshot_ch_telemetry_s telemetry[DSHOT_NCHANNELS];
   bool bidir;
 };

 /****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/

 static int dshot_open(FAR struct file *filep);
 static int dshot_close(FAR struct file *filep);
 static ssize_t dshot_read(FAR struct file *filep, FAR char *buffer,
                           size_t buflen);
 static ssize_t dshot_write(FAR struct file *filep, FAR const char *buffer,
                            size_t buflen);
 static int dshot_ioctl(FAR struct file *filep, int cmd, unsigned long arg);

 /****************************************************************************
  * Private Data
  ****************************************************************************/

 static const struct file_operations g_dshotops =
 {
   dshot_open,  /* open */
   dshot_close, /* close */
   dshot_read,  /* read */
   dshot_write, /* write */
   NULL,        /* seek */
   dshot_ioctl, /* ioctl */
 };

 /****************************************************************************
  * Private Functions
  ****************************************************************************/

 /****************************************************************************
  * Name: dshot_crc
  *
  * Description:
  *   Calculate the 4-bit CRC for a DShot packet.
  *
  * Input Parameters:
  *   packet - The DShot packet to calculate CRC for.
  *   bidir  - Boolean indicating if bidirectional DShot is enabled.
  *
  * Returned Value:
  *   The calculated 4-bit CRC value.
  *
  ****************************************************************************/

 static uint16_t dshot_crc(uint16_t packet, bool bidir)
 {
   uint16_t crc;

   crc = packet >> 4;
   crc = crc ^ (crc >> 4) ^ (crc >> 8);

   if (bidir)
     {
       crc = ~crc;
     }

   return crc & 0xf;
 }

 /****************************************************************************
  * Name: dshot_build_packet
  *
  * Description:
  *   Build a complete DShot packet from throttle, telemetry, and bidir
  *   values.
  *
  * Input Parameters:
  *   throttle   - Throttle value (0-2047).
  *   telemetry  - Boolean indicating if telemetry request is enabled.
  *   bidir      - Boolean indicating if bidirectional DShot is enabled.
  *
  * Returned Value:
  *   The constructed 16-bit DShot packet.
  *
  ****************************************************************************/

 static uint16_t dshot_build_packet(uint16_t throttle, bool telemetry,
                                    bool bidir)
 {
   uint16_t packet = 0;
   packet |= throttle << 5;
   packet |= (telemetry ? 1 << 4 : 0);
   packet |= dshot_crc(packet, bidir);

   return packet;
 }

 /****************************************************************************
  * Name: dshot_parse_raw_packet
  *
  * Description:
  *   Parse raw pattern from the line into a 16-bit packet and verify CRC.
  *   Steps:
  *   1. Extract GCR20 value from the raw value
  *   2. Extract the 16-bit packet from the GCR20
  *   3. Check the packet CRC
  *
  * Input Parameters:
  *   raw - The raw encoded packet value to parse.
  *
  * Returned Value:
  *   On success, returns the 12-bit payload as a positive integer.
  *   On error, returns a negative error code.
  *
  ****************************************************************************/

 static int dshot_parse_raw_packet(uint32_t raw)
 {
   static const int8_t gcr_decode[32] =
     {
       -1, -1, -1, -1, -1, -1, -1, -1,
       -1,  9, 10, 11, -1, 13, 14, 15,
       -1, -1,  2,  3, -1,  5,  6,  7,
       -1,  0,  8,  1, -1,  4, 12, -1
     };

   uint32_t gcr20 = (raw ^ (raw >> 1)) & DSHOT_GCR_MASK;
   uint16_t packet = 0;
   int i;

   for (i = 0; i < 4; i++)
     {
       int16_t nibble = gcr_decode[gcr20 & 0x1f];

       if (nibble < 0)
         {
           return -EINVAL;
         }

       packet |= nibble << (4 * i);
       gcr20 >>= 5;
     }

   if (dshot_crc(packet, true) != (packet & 0xf))
     {
       return -EINVAL;
     }

   return packet >> 4;
 }

 /****************************************************************************
  * Name: dshot_copy_telemetry
  *
  * Description:
  *   Copy telemetry data for selected channels from upper-half to output.
  *
  * Input Parameters:
  *   upper   - Pointer to the upper-half driver state.
  *   out     - Pointer to output telemetry array.
  *   ch_mask - Bitmask indicating which channels to copy.
  *
  * Returned Value:
  *   None.
  *
  ****************************************************************************/

 static void dshot_copy_telemetry(FAR struct dshot_upperhalf_s *upper,
                                  FAR struct dshot_ch_telemetry_s *out,
                                  uint16_t ch_mask)
 {
   int i;

   for (i = 0; i < DSHOT_NCHANNELS; i++)
     {
       if ((ch_mask & (1u << i)) != 0)
         {
           out[i] = upper->telemetry[i];
         }
     }
 }

 /****************************************************************************
  * Name: dshot_update_telemetry
  *
  * Description:
  *   Update telemetry data from lower-half driver for selected channels.
  *   Parses raw telemetry data and extracts EDT or eRPM information.
  *
  * Input Parameters:
  *   upper   - Pointer to the upper-half driver state.
  *   ch_mask - Bitmask indicating which channels to update.
  *
  * Returned Value:
  *   OK on success; a negated errno value on failure.
  *
  ****************************************************************************/

 static int dshot_update_telemetry(FAR struct dshot_upperhalf_s *upper,
                                   uint16_t ch_mask)
 {
   FAR struct dshot_lowerhalf_s *lower = upper->dev;
   struct dshot_raw_telemetry_s raw[DSHOT_NCHANNELS];
   int i;
   int ret;

   memset(raw, 0, sizeof(raw));

   DEBUGASSERT(lower->ops->get_raw_telemetry != NULL);
   ret = lower->ops->get_raw_telemetry(lower, raw, ch_mask);
   if (ret < 0)
     {
       return ret;
     }

   for (i = 0; i < DSHOT_NCHANNELS; i++)
     {
       if ((ch_mask & (1u << i)) != 0)
         {
           int value = dshot_parse_raw_packet(raw[i].raw);

           if (value > 0)
             {
               uint16_t telem = (uint16_t)value & 0x0fff;
               uint8_t exponent = telem >> 9;

               if ((exponent & 1) == 0)
                 {
                   upper->telemetry[i].edt_type = (uint8_t)(telem >> 8);
                   upper->telemetry[i].edt_value = (uint8_t)(telem & 0xff);
                   upper->telemetry[i].timestamp = raw[i].timestamp;
                 }
               else
                 {
                   uint32_t mantissa = telem & 0x1ff;
                   uint32_t period = mantissa << exponent;
                   if (period != 0)
                     {
                       uint32_t rpm = DSHOT_ERPMSCALE_NUM / period;
                       upper->telemetry[i].erpm =
                         (uint16_t)((rpm > 0xffff) ? 0xffff : rpm);
                       upper->telemetry[i].timestamp = raw[i].timestamp;
                     }
                 }
             }
         }
     }

   return OK;
 }

 /****************************************************************************
  * Name: dshot_open
  *
  * Description:
  *   This function is called whenever the DShot device is opened.
  *
  * Input Parameters:
  *   filep - A pointer to the file structure instance.
  *
  * Returned Value:
  *   OK on success; a negated errno value on failure.
  *
  ****************************************************************************/

 static int dshot_open(FAR struct file *filep)
 {
   FAR struct inode *inode = filep->f_inode;
   FAR struct dshot_upperhalf_s *upper = inode->i_private;
   uint8_t tmp;
   int ret;

   ret = nxmutex_lock(&upper->lock);
   if (ret < 0)
     {
       return ret;
     }

   tmp = upper->crefs + 1;
   if (tmp == 0)
     {
       nxmutex_unlock(&upper->lock);
       return -EMFILE;
     }

   if (tmp == 1)
     {
       DEBUGASSERT(upper->dev->ops->setup != NULL);
       ret = upper->dev->ops->setup(upper->dev);
       if (ret < 0)
         {
           nxmutex_unlock(&upper->lock);
           return ret;
         }
     }

   upper->crefs = tmp;

   nxmutex_unlock(&upper->lock);
   return OK;
 }

 /****************************************************************************
  * Name: dshot_close
  *
  * Description:
  *   This function is called when the DShot device is closed.
  *
  * Input Parameters:
  *   filep - A pointer to the file structure instance.
  *
  * Returned Value:
  *   OK on success; a negated errno value on failure.
  *
  ****************************************************************************/

 static int dshot_close(FAR struct file *filep)
 {
   FAR struct inode *inode = filep->f_inode;
   FAR struct dshot_upperhalf_s *upper = inode->i_private;
   int ret;

   ret = nxmutex_lock(&upper->lock);
   if (ret < 0)
     {
       return ret;
     }

   if (upper->crefs > 1)
     {
       upper->crefs--;
     }
   else
     {
       upper->crefs = 0;
       DEBUGASSERT(upper->dev->ops->shutdown != NULL);
       upper->dev->ops->shutdown(upper->dev);
     }

   nxmutex_unlock(&upper->lock);
   return OK;
 }

 /****************************************************************************
  * Name: dshot_read
  *
  * Description:
  *   A dummy read method. This is provided only to satisfy the VFS layer.
  *
  * Input Parameters:
  *   filep  - A pointer to the file structure instance.
  *   buffer - The user-provided buffer into which data will be returned.
  *   buflen - The size of the buffer in bytes.
  *
  * Returned Value:
  *   Always returns 0 (end-of-file).
  *
  ****************************************************************************/

 static ssize_t dshot_read(FAR struct file *filep, FAR char *buffer,
                           size_t buflen)
 {
   return 0;
 }

 /****************************************************************************
  * Name: dshot_write
  *
  * Description:
  *   A dummy write method. This is provided only to satisfy the VFS layer.
  *
  * Input Parameters:
  *   filep  - A pointer to the file structure instance.
  *   buffer - The user-provided buffer from which data will be written.
  *   buflen - The number of bytes to be written.
  *
  * Returned Value:
  *   Always returns -EPERM (operation not permitted).
  *
  ****************************************************************************/

 static ssize_t dshot_write(FAR struct file *filep, FAR const char *buffer,
                            size_t buflen)
 {
   return -EPERM;
 }

 /****************************************************************************
  * Name: dshot_ioctl
  *
  * Description:
  *   The standard ioctl method. This is used to perform DShot-specific
  *   operations including setting throttle, configuring channels, and
  *   retrieving telemetry data.
  *
  * Input Parameters:
  *   filep - A pointer to the file structure instance.
  *   cmd   - The ioctl command code.
  *   arg   - The argument provided with the ioctl command.
  *
  * Returned Value:
  *   OK on success; a negated errno value on failure.
  *
  ****************************************************************************/

 static int dshot_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
 {
   FAR struct inode *inode = filep->f_inode;
   FAR struct dshot_upperhalf_s *upper = inode->i_private;
   FAR struct dshot_lowerhalf_s *lower = upper->dev;
   int ret;

   ret = nxmutex_lock(&upper->lock);
   if (ret < 0)
     {
       return ret;
     }

   switch (cmd)
     {
       case DSHOTIOC_CONFIGURE:
         {
           FAR struct dshot_config_s *cfg =
             (FAR struct dshot_config_s *)((uintptr_t)arg);

           if (cfg == NULL)
             {
               ret = -EINVAL;
               break;
             }

           DEBUGASSERT(lower->ops->configure);
           ret = lower->ops->configure(lower, cfg);
           upper->bidir = cfg->bidir;
         }
         break;

       case DSHOTIOC_SET_THROTTLE:
         {
           FAR struct dshot_throttle_s *req =
             (FAR struct dshot_throttle_s *)((uintptr_t)arg);
           uint16_t packets[DSHOT_NCHANNELS];
           int i;

           if (req == NULL)
             {
               ret = -EINVAL;
               break;
             }

           /* Retrieve the latest telemetry */

           if (req->telemetry_req != 0)
             {
               ret = dshot_update_telemetry(upper, req->telemetry_req);
               if (ret < 0)
                 {
                   break;
                 }

               dshot_copy_telemetry(upper, req->ch_telemetry,
                                    req->telemetry_req);
             }

           /* Build DShot packets for each channel */

           for (i = 0; i < DSHOT_NCHANNELS; i++)
             {
               if ((req->ch_mask & (1u << i)) != 0)
                 {
                   bool telemetry = (req->telemetry_req & (1u << i)) != 0;
                   packets[i] =
                     dshot_build_packet(req->throttle[i], telemetry,
                                        upper->bidir);
                 }
             }

           /* Send the packets */

           DEBUGASSERT(lower->ops->send_command);
           ret = lower->ops->send_command(lower, packets, req->ch_mask);
         }
         break;

       case DSHOTIOC_GET_TELEMETRY:
         {
           FAR struct dshot_telemetry_s *tlm =
             (FAR struct dshot_telemetry_s *)((uintptr_t)arg);

           if (tlm == NULL)
             {
               ret = -EINVAL;
               break;
             }

           ret = dshot_update_telemetry(upper, tlm->ch_mask);
           if (ret < 0)
             {
               break;
             }

           dshot_copy_telemetry(upper, tlm->ch_telemetry, tlm->ch_mask);
           ret = OK;
         }
         break;

       default:
         {
           if (lower->ops->ioctl != NULL)
             {
               ret = lower->ops->ioctl(lower, cmd, arg);
             }
           else
             {
               ret = -ENOTTY;
             }
         }
         break;
     }

   nxmutex_unlock(&upper->lock);
   return ret;
 }

 /****************************************************************************
  * Public Functions
  ****************************************************************************/

 /****************************************************************************
  * Name: dshot_register
  *
  * Description:
  *   This function binds an instance of a "lower half" DShot driver with the
  *   "upper half" DShot device and registers that device so that it can be
  *   used by application code.
  *
  *   When this function is called, the "lower half" driver should be in the
  *   reset state (as if the shutdown() method had already been called).
  *
  * Input Parameters:
  *   path - The full path to the driver to be registered in the NuttX pseudo-
  *     filesystem. The recommended convention is to name all DShot drivers
  *     as "/dev/dshot0", "/dev/dshot1", etc., where the driver path differs
  *     only in the "minor" number at the end of the device name.
  *   dev  - A pointer to an instance of lower half DShot driver. This
  *     instance is bound to the DShot driver and must persist as long as the
  *     driver persists.
  *
  * Returned Value:
  *   Zero on success; a negated errno value on failure.
  *
  ****************************************************************************/

 int dshot_register(FAR const char *path, FAR struct dshot_lowerhalf_s *dev)
 {
   FAR struct dshot_upperhalf_s *upper;
   int ret;

   if (dev == NULL || dev->ops == NULL)
     {
       return -EINVAL;
     }

   upper = kmm_zalloc(sizeof(struct dshot_upperhalf_s));
   if (upper == NULL)
     {
       return -ENOMEM;
     }

   nxmutex_init(&upper->lock);
   upper->dev = dev;

   ret = register_driver(path, &g_dshotops, 0600, upper);
   if (ret < 0)
     {
       nxmutex_destroy(&upper->lock);
       kmm_free(upper);
       return ret;
     }

   return OK;
 }

 #endif /* CONFIG_DSHOT */
	/****************************************************************************
	* drivers/timers/dshot.c
	*
	* SPDX-License-Identifier: Apache-2.0
	*
	* Licensed to the Apache Software Foundation (ASF) under one or more
	* contributor license agreements. See the NOTICE file distributed with
	* this work for additional information regarding copyright ownership. The
	* ASF licenses this file to you under the Apache License, Version 2.0 (the
	* "License"); you may not use this file except in compliance with the
	* License. You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
	* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
	* License for the specific language governing permissions and limitations
	* under the License.
	*
	****************************************************************************/

	/****************************************************************************
	* Included Files
	****************************************************************************/

	#include <nuttx/config.h>

	#include <sys/types.h>
	#include <stdint.h>
	#include <stdbool.h>
	#include <stdlib.h>
	#include <string.h>
	#include <errno.h>
	#include <debug.h>

	#include <nuttx/fs/fs.h>
	#include <nuttx/kmalloc.h>
	#include <nuttx/mutex.h>
	#include <nuttx/timers/dshot.h>

	#ifdef CONFIG_DSHOT

	/****************************************************************************
	* Pre-processor Definitions
	****************************************************************************/

	#define DSHOT_GCR_MASK 0x000fffffu
	#define DSHOT_ERPMSCALE_NUM 600000u

	/****************************************************************************
	* Private Types
	****************************************************************************/

	struct dshot_upperhalf_s
	{
	uint8_t crefs;
	mutex_t lock;
	FAR struct dshot_lowerhalf_s *dev;
	struct dshot_ch_telemetry_s telemetry[DSHOT_NCHANNELS];
	bool bidir;
	};

	/****************************************************************************
	* Private Function Prototypes
	****************************************************************************/

	static int dshot_open(FAR struct file *filep);
	static int dshot_close(FAR struct file *filep);
	static ssize_t dshot_read(FAR struct file filep, FAR char buffer,
	size_t buflen);
	static ssize_t dshot_write(FAR struct file filep, FAR const char buffer,
	size_t buflen);
	static int dshot_ioctl(FAR struct file *filep, int cmd, unsigned long arg);

	/****************************************************************************
	* Private Data
	****************************************************************************/

	static const struct file_operations g_dshotops =
	{
	dshot_open, /* open */
	dshot_close, /* close */
	dshot_read, /* read */
	dshot_write, /* write */
	NULL, /* seek */
	dshot_ioctl, /* ioctl */
	};

	/****************************************************************************
	* Private Functions
	****************************************************************************/

	/****************************************************************************
	* Name: dshot_crc
	*
	* Description:
	* Calculate the 4-bit CRC for a DShot packet.
	*
	* Input Parameters:
	* packet - The DShot packet to calculate CRC for.
	* bidir - Boolean indicating if bidirectional DShot is enabled.
	*
	* Returned Value:
	* The calculated 4-bit CRC value.
	*
	****************************************************************************/

	static uint16_t dshot_crc(uint16_t packet, bool bidir)
	{
	uint16_t crc;

	crc = packet >> 4;
	crc = crc ^ (crc >> 4) ^ (crc >> 8);

	if (bidir)
	{
	crc = ~crc;
	}

	return crc & 0xf;
	}

	/****************************************************************************
	* Name: dshot_build_packet
	*
	* Description:
	* Build a complete DShot packet from throttle, telemetry, and bidir
	* values.
	*
	* Input Parameters:
	* throttle - Throttle value (0-2047).
	* telemetry - Boolean indicating if telemetry request is enabled.
	* bidir - Boolean indicating if bidirectional DShot is enabled.
	*
	* Returned Value:
	* The constructed 16-bit DShot packet.
	*
	****************************************************************************/

	static uint16_t dshot_build_packet(uint16_t throttle, bool telemetry,
	bool bidir)
	{
	uint16_t packet = 0;
	packet \|= throttle << 5;
	packet \|= (telemetry ? 1 << 4 : 0);
	packet \|= dshot_crc(packet, bidir);

	return packet;
	}

	/****************************************************************************
	* Name: dshot_parse_raw_packet
	*
	* Description:
	* Parse raw pattern from the line into a 16-bit packet and verify CRC.
	* Steps:
	* 1. Extract GCR20 value from the raw value
	* 2. Extract the 16-bit packet from the GCR20
	* 3. Check the packet CRC
	*
	* Input Parameters:
	* raw - The raw encoded packet value to parse.
	*
	* Returned Value:
	* On success, returns the 12-bit payload as a positive integer.
	* On error, returns a negative error code.
	*
	****************************************************************************/

	static int dshot_parse_raw_packet(uint32_t raw)
	{
	static const int8_t gcr_decode[32] =
	{
	-1, -1, -1, -1, -1, -1, -1, -1,
	-1, 9, 10, 11, -1, 13, 14, 15,
	-1, -1, 2, 3, -1, 5, 6, 7,
	-1, 0, 8, 1, -1, 4, 12, -1
	};

	uint32_t gcr20 = (raw ^ (raw >> 1)) & DSHOT_GCR_MASK;
	uint16_t packet = 0;
	int i;

	for (i = 0; i < 4; i++)
	{
	int16_t nibble = gcr_decode[gcr20 & 0x1f];

	if (nibble < 0)
	{
	return -EINVAL;
	}

	packet \|= nibble << (4 * i);
	gcr20 >>= 5;
	}

	if (dshot_crc(packet, true) != (packet & 0xf))
	{
	return -EINVAL;
	}

	return packet >> 4;
	}

	/****************************************************************************
	* Name: dshot_copy_telemetry
	*
	* Description:
	* Copy telemetry data for selected channels from upper-half to output.
	*
	* Input Parameters:
	* upper - Pointer to the upper-half driver state.
	* out - Pointer to output telemetry array.
	* ch_mask - Bitmask indicating which channels to copy.
	*
	* Returned Value:
	* None.
	*
	****************************************************************************/

	static void dshot_copy_telemetry(FAR struct dshot_upperhalf_s *upper,
	FAR struct dshot_ch_telemetry_s *out,
	uint16_t ch_mask)
	{
	int i;

	for (i = 0; i < DSHOT_NCHANNELS; i++)
	{
	if ((ch_mask & (1u << i)) != 0)
	{
	out[i] = upper->telemetry[i];
	}
	}
	}

	/****************************************************************************
	* Name: dshot_update_telemetry
	*
	* Description:
	* Update telemetry data from lower-half driver for selected channels.
	* Parses raw telemetry data and extracts EDT or eRPM information.
	*
	* Input Parameters:
	* upper - Pointer to the upper-half driver state.
	* ch_mask - Bitmask indicating which channels to update.
	*
	* Returned Value:
	* OK on success; a negated errno value on failure.
	*
	****************************************************************************/

	static int dshot_update_telemetry(FAR struct dshot_upperhalf_s *upper,
	uint16_t ch_mask)
	{
	FAR struct dshot_lowerhalf_s *lower = upper->dev;
	struct dshot_raw_telemetry_s raw[DSHOT_NCHANNELS];
	int i;
	int ret;

	memset(raw, 0, sizeof(raw));

	DEBUGASSERT(lower->ops->get_raw_telemetry != NULL);
	ret = lower->ops->get_raw_telemetry(lower, raw, ch_mask);
	if (ret < 0)
	{
	return ret;
	}

	for (i = 0; i < DSHOT_NCHANNELS; i++)
	{
	if ((ch_mask & (1u << i)) != 0)
	{
	int value = dshot_parse_raw_packet(raw[i].raw);

	if (value > 0)
	{
	uint16_t telem = (uint16_t)value & 0x0fff;
	uint8_t exponent = telem >> 9;

	if ((exponent & 1) == 0)
	{
	upper->telemetry[i].edt_type = (uint8_t)(telem >> 8);
	upper->telemetry[i].edt_value = (uint8_t)(telem & 0xff);
	upper->telemetry[i].timestamp = raw[i].timestamp;
	}
	else
	{
	uint32_t mantissa = telem & 0x1ff;
	uint32_t period = mantissa << exponent;
	if (period != 0)
	{
	uint32_t rpm = DSHOT_ERPMSCALE_NUM / period;
	upper->telemetry[i].erpm =
	(uint16_t)((rpm > 0xffff) ? 0xffff : rpm);
	upper->telemetry[i].timestamp = raw[i].timestamp;
	}
	}
	}
	}
	}

	return OK;
	}

	/****************************************************************************
	* Name: dshot_open
	*
	* Description:
	* This function is called whenever the DShot device is opened.
	*
	* Input Parameters:
	* filep - A pointer to the file structure instance.
	*
	* Returned Value:
	* OK on success; a negated errno value on failure.
	*
	****************************************************************************/

	static int dshot_open(FAR struct file *filep)
	{
	FAR struct inode *inode = filep->f_inode;
	FAR struct dshot_upperhalf_s *upper = inode->i_private;
	uint8_t tmp;
	int ret;

	ret = nxmutex_lock(&upper->lock);
	if (ret < 0)
	{
	return ret;
	}

	tmp = upper->crefs + 1;
	if (tmp == 0)
	{
	nxmutex_unlock(&upper->lock);
	return -EMFILE;
	}

	if (tmp == 1)
	{
	DEBUGASSERT(upper->dev->ops->setup != NULL);
	ret = upper->dev->ops->setup(upper->dev);
	if (ret < 0)
	{
	nxmutex_unlock(&upper->lock);
	return ret;
	}
	}

	upper->crefs = tmp;

	nxmutex_unlock(&upper->lock);
	return OK;
	}

	/****************************************************************************
	* Name: dshot_close
	*
	* Description:
	* This function is called when the DShot device is closed.
	*
	* Input Parameters:
	* filep - A pointer to the file structure instance.
	*
	* Returned Value:
	* OK on success; a negated errno value on failure.
	*
	****************************************************************************/

	static int dshot_close(FAR struct file *filep)
	{
	FAR struct inode *inode = filep->f_inode;
	FAR struct dshot_upperhalf_s *upper = inode->i_private;
	int ret;

	ret = nxmutex_lock(&upper->lock);
	if (ret < 0)
	{
	return ret;
	}

	if (upper->crefs > 1)
	{
	upper->crefs--;
	}
	else
	{
	upper->crefs = 0;
	DEBUGASSERT(upper->dev->ops->shutdown != NULL);
	upper->dev->ops->shutdown(upper->dev);
	}

	nxmutex_unlock(&upper->lock);
	return OK;
	}

	/****************************************************************************
	* Name: dshot_read
	*
	* Description:
	* A dummy read method. This is provided only to satisfy the VFS layer.
	*
	* Input Parameters:
	* filep - A pointer to the file structure instance.
	* buffer - The user-provided buffer into which data will be returned.
	* buflen - The size of the buffer in bytes.
	*
	* Returned Value:
	* Always returns 0 (end-of-file).
	*
	****************************************************************************/

	static ssize_t dshot_read(FAR struct file filep, FAR char buffer,
	size_t buflen)
	{
	return 0;
	}

	/****************************************************************************
	* Name: dshot_write
	*
	* Description:
	* A dummy write method. This is provided only to satisfy the VFS layer.
	*
	* Input Parameters:
	* filep - A pointer to the file structure instance.
	* buffer - The user-provided buffer from which data will be written.
	* buflen - The number of bytes to be written.
	*
	* Returned Value:
	* Always returns -EPERM (operation not permitted).
	*
	****************************************************************************/

	static ssize_t dshot_write(FAR struct file filep, FAR const char buffer,
	size_t buflen)
	{
	return -EPERM;
	}

	/****************************************************************************
	* Name: dshot_ioctl
	*
	* Description:
	* The standard ioctl method. This is used to perform DShot-specific
	* operations including setting throttle, configuring channels, and
	* retrieving telemetry data.
	*
	* Input Parameters:
	* filep - A pointer to the file structure instance.
	* cmd - The ioctl command code.
	* arg - The argument provided with the ioctl command.
	*
	* Returned Value:
	* OK on success; a negated errno value on failure.
	*
	****************************************************************************/

	static int dshot_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
	{
	FAR struct inode *inode = filep->f_inode;
	FAR struct dshot_upperhalf_s *upper = inode->i_private;
	FAR struct dshot_lowerhalf_s *lower = upper->dev;
	int ret;

	ret = nxmutex_lock(&upper->lock);
	if (ret < 0)
	{
	return ret;
	}

	switch (cmd)
	{
	case DSHOTIOC_CONFIGURE:
	{
	FAR struct dshot_config_s *cfg =
	(FAR struct dshot_config_s *)((uintptr_t)arg);

	if (cfg == NULL)
	{
	ret = -EINVAL;
	break;
	}

	DEBUGASSERT(lower->ops->configure);
	ret = lower->ops->configure(lower, cfg);
	upper->bidir = cfg->bidir;
	}
	break;

	case DSHOTIOC_SET_THROTTLE:
	{
	FAR struct dshot_throttle_s *req =
	(FAR struct dshot_throttle_s *)((uintptr_t)arg);
	uint16_t packets[DSHOT_NCHANNELS];
	int i;

	if (req == NULL)
	{
	ret = -EINVAL;
	break;
	}

	/* Retrieve the latest telemetry */

	if (req->telemetry_req != 0)
	{
	ret = dshot_update_telemetry(upper, req->telemetry_req);
	if (ret < 0)
	{
	break;
	}

	dshot_copy_telemetry(upper, req->ch_telemetry,
	req->telemetry_req);
	}

	/* Build DShot packets for each channel */

	for (i = 0; i < DSHOT_NCHANNELS; i++)
	{
	if ((req->ch_mask & (1u << i)) != 0)
	{
	bool telemetry = (req->telemetry_req & (1u << i)) != 0;
	packets[i] =
	dshot_build_packet(req->throttle[i], telemetry,
	upper->bidir);
	}
	}

	/* Send the packets */

	DEBUGASSERT(lower->ops->send_command);
	ret = lower->ops->send_command(lower, packets, req->ch_mask);
	}
	break;

	case DSHOTIOC_GET_TELEMETRY:
	{
	FAR struct dshot_telemetry_s *tlm =
	(FAR struct dshot_telemetry_s *)((uintptr_t)arg);

	if (tlm == NULL)
	{
	ret = -EINVAL;
	break;
	}

	ret = dshot_update_telemetry(upper, tlm->ch_mask);
	if (ret < 0)
	{
	break;
	}

	dshot_copy_telemetry(upper, tlm->ch_telemetry, tlm->ch_mask);
	ret = OK;
	}
	break;

	default:
	{
	if (lower->ops->ioctl != NULL)
	{
	ret = lower->ops->ioctl(lower, cmd, arg);
	}
	else
	{
	ret = -ENOTTY;
	}
	}
	break;
	}

	nxmutex_unlock(&upper->lock);
	return ret;
	}

	/****************************************************************************
	* Public Functions
	****************************************************************************/

	/****************************************************************************
	* Name: dshot_register
	*
	* Description:
	* This function binds an instance of a "lower half" DShot driver with the
	* "upper half" DShot device and registers that device so that it can be
	* used by application code.
	*
	* When this function is called, the "lower half" driver should be in the
	* reset state (as if the shutdown() method had already been called).
	*
	* Input Parameters:
	* path - The full path to the driver to be registered in the NuttX pseudo-
	* filesystem. The recommended convention is to name all DShot drivers
	* as "/dev/dshot0", "/dev/dshot1", etc., where the driver path differs
	* only in the "minor" number at the end of the device name.
	* dev - A pointer to an instance of lower half DShot driver. This
	* instance is bound to the DShot driver and must persist as long as the
	* driver persists.
	*
	* Returned Value:
	* Zero on success; a negated errno value on failure.
	*
	****************************************************************************/

	int dshot_register(FAR const char path, FAR struct dshot_lowerhalf_s dev)
	{
	FAR struct dshot_upperhalf_s *upper;
	int ret;

	if (dev == NULL \|\| dev->ops == NULL)
	{
	return -EINVAL;
	}

	upper = kmm_zalloc(sizeof(struct dshot_upperhalf_s));
	if (upper == NULL)
	{
	return -ENOMEM;
	}

	nxmutex_init(&upper->lock);
	upper->dev = dev;

	ret = register_driver(path, &g_dshotops, 0600, upper);
	if (ret < 0)
	{
	nxmutex_destroy(&upper->lock);
	kmm_free(upper);
	return ret;
	}

	return OK;
	}

	#endif /* CONFIG_DSHOT */