Line data    Source code

       1             : #include <AP_HAL/utility/sparse-endian.h>
       2             : #include <AP_InternalError/AP_InternalError.h>
       3             : 
       4             : #include "msp.h"
       5             : #include "msp_sbuf.h"
       6             : 
       7             : #include <string.h>
       8             : 
       9             : #if HAL_MSP_ENABLED
      10             : 
      11       43944 : uint8_t* MSP::sbuf_ptr(sbuf_t *buf)
      12             : {
      13       43944 :     return buf->ptr;
      14             : }
      15             : 
      16       44036 : uint16_t MSP::sbuf_bytes_remaining(const sbuf_t *buf)
      17             : {
      18       44036 :     return buf->end - buf->ptr;
      19             : }
      20             : 
      21       22008 : bool MSP::sbuf_check_bounds(const sbuf_t *buf, const uint8_t len)
      22             : {
      23       22008 :     if (sbuf_bytes_remaining(buf) < len) {
      24           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      25           0 :         return false;
      26             :     }
      27       22008 :     return true;
      28             : }
      29             : 
      30       21972 : void MSP::sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base)
      31             : {
      32       21972 :     buf->end = buf->ptr;
      33       21972 :     buf->ptr = base;
      34       21972 : }
      35             : 
      36       21956 : void MSP::sbuf_write_data(sbuf_t *dst, const void *data, int len)
      37             : {
      38       21956 :     if (!sbuf_check_bounds(dst, len)) {
      39           0 :         return;
      40             :     }
      41       21956 :     memcpy(dst->ptr, data, len);
      42       21956 :     dst->ptr += len;
      43             : }
      44             : 
      45          32 : uint8_t MSP::sbuf_read_u8(sbuf_t *src)
      46             : {
      47          32 :     if (!sbuf_check_bounds(src, 1)) {
      48           0 :         return 0;
      49             :     }
      50          32 :     return *src->ptr++;
      51             : }
      52             : 
      53          20 : uint16_t MSP::sbuf_read_u16(sbuf_t *src)
      54             : {
      55          20 :     if (!sbuf_check_bounds(src, 2)) {
      56           0 :         return 0;
      57             :     }
      58          20 :     uint16_t val = le16toh_ptr(src->ptr);
      59          20 :     src->ptr += 2;
      60          20 :     return val;
      61             : }
      62             : 
      63             : #endif //HAL_MSP_ENABLED