From cee9a2e926adb12f422138f46c2213c1c5576e29 Mon Sep 17 00:00:00 2001 From: Jef Driesen Date: Mon, 20 Mar 2023 21:29:27 +0100 Subject: [PATCH] Add a generic HDLC I/O implementation The new HDLC I/O provides a generic I/O stream for reading and writing HDLC encoded frames on top of another base I/O stream. --- contrib/android/Android.mk | 1 + contrib/msvc/libdivecomputer.vcxproj | 2 + src/Makefile.am | 1 + src/hdlc.c | 406 +++++++++++++++++++++++++++ src/hdlc.h | 50 ++++ 5 files changed, 460 insertions(+) create mode 100644 src/hdlc.c create mode 100644 src/hdlc.h diff --git a/contrib/android/Android.mk b/contrib/android/Android.mk index 61961a0..d2d8cd1 100644 --- a/contrib/android/Android.mk +++ b/contrib/android/Android.mk @@ -36,6 +36,7 @@ LOCAL_SRC_FILES := \ src/diverite_nitekq_parser.c \ src/divesystem_idive.c \ src/divesystem_idive_parser.c \ + src/hdlc.c \ src/hw_frog.c \ src/hw_ostc3.c \ src/hw_ostc.c \ diff --git a/contrib/msvc/libdivecomputer.vcxproj b/contrib/msvc/libdivecomputer.vcxproj index 406c577..6497705 100644 --- a/contrib/msvc/libdivecomputer.vcxproj +++ b/contrib/msvc/libdivecomputer.vcxproj @@ -204,6 +204,7 @@ + @@ -331,6 +332,7 @@ + diff --git a/src/Makefile.am b/src/Makefile.am index 9234ba7..1b37d9c 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -80,6 +80,7 @@ libdivecomputer_la_SOURCES = \ deepblu_cosmiq.h deepblu_cosmiq.c deepblu_cosmiq_parser.c \ oceans_s1_common.h oceans_s1_common.c \ oceans_s1.h oceans_s1.c oceans_s1_parser.c \ + hdlc.h hdlc.c \ socket.h socket.c \ irda.c \ usb.c \ diff --git a/src/hdlc.c b/src/hdlc.c new file mode 100644 index 0000000..40e1ae7 --- /dev/null +++ b/src/hdlc.c @@ -0,0 +1,406 @@ +/* + * libdivecomputer + * + * Copyright (C) 2023 Jef Driesen + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301 USA + */ + +#include // malloc, free + +#include "hdlc.h" + +#include "iostream-private.h" +#include "common-private.h" +#include "context-private.h" + +#define END 0x7E +#define ESC 0x7D +#define ESC_BIT 0x20 + +static dc_status_t dc_hdlc_set_timeout (dc_iostream_t *abstract, int timeout); +static dc_status_t dc_hdlc_set_break (dc_iostream_t *abstract, unsigned int value); +static dc_status_t dc_hdlc_set_dtr (dc_iostream_t *abstract, unsigned int value); +static dc_status_t dc_hdlc_set_rts (dc_iostream_t *abstract, unsigned int value); +static dc_status_t dc_hdlc_get_lines (dc_iostream_t *abstract, unsigned int *value); +static dc_status_t dc_hdlc_configure (dc_iostream_t *abstract, unsigned int baudrate, unsigned int databits, dc_parity_t parity, dc_stopbits_t stopbits, dc_flowcontrol_t flowcontrol); +static dc_status_t dc_hdlc_poll (dc_iostream_t *abstract, int timeout); +static dc_status_t dc_hdlc_read (dc_iostream_t *abstract, void *data, size_t size, size_t *actual); +static dc_status_t dc_hdlc_write (dc_iostream_t *abstract, const void *data, size_t size, size_t *actual); +static dc_status_t dc_hdlc_ioctl (dc_iostream_t *abstract, unsigned int request, void *data, size_t size); +static dc_status_t dc_hdlc_flush (dc_iostream_t *abstract); +static dc_status_t dc_hdlc_purge (dc_iostream_t *abstract, dc_direction_t direction); +static dc_status_t dc_hdlc_sleep (dc_iostream_t *abstract, unsigned int milliseconds); +static dc_status_t dc_hdlc_close (dc_iostream_t *abstract); + +typedef struct dc_hdlc_t { + /* Base class. */ + dc_iostream_t base; + /* Internal state. */ + dc_context_t *context; + dc_iostream_t *iostream; + unsigned char *rbuf; + unsigned char *wbuf; + size_t rbuf_size; + size_t rbuf_offset; + size_t rbuf_available; + size_t wbuf_size; + size_t wbuf_offset; +} dc_hdlc_t; + +static const dc_iostream_vtable_t dc_hdlc_vtable = { + sizeof(dc_hdlc_t), + dc_hdlc_set_timeout, /* set_timeout */ + dc_hdlc_set_break, /* set_break */ + dc_hdlc_set_dtr, /* set_dtr */ + dc_hdlc_set_rts, /* set_rts */ + dc_hdlc_get_lines, /* get_lines */ + NULL, /* get_available */ + dc_hdlc_configure, /* configure */ + dc_hdlc_poll, /* poll */ + dc_hdlc_read, /* read */ + dc_hdlc_write, /* write */ + dc_hdlc_ioctl, /* ioctl */ + dc_hdlc_flush, /* flush */ + dc_hdlc_purge, /* purge */ + dc_hdlc_sleep, /* sleep */ + dc_hdlc_close, /* close */ +}; + +dc_status_t +dc_hdlc_open (dc_iostream_t **out, dc_context_t *context, dc_iostream_t *base, size_t isize, size_t osize) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_hdlc_t *hdlc = NULL; + + if (out == NULL) + return DC_STATUS_INVALIDARGS; + + if (base == NULL || isize == 0 || osize == 0) + return DC_STATUS_INVALIDARGS; + + dc_transport_t transport = dc_iostream_get_transport (base); + + // Allocate memory. + hdlc = (dc_hdlc_t *) dc_iostream_allocate (NULL, &dc_hdlc_vtable, transport); + if (hdlc == NULL) { + ERROR (context, "Failed to allocate memory."); + status = DC_STATUS_NOMEMORY; + goto error_exit; + } + + // Allocate the read buffer. + hdlc->rbuf = malloc (isize); + if (hdlc->rbuf == NULL) { + ERROR (context, "Failed to allocate memory."); + status = DC_STATUS_NOMEMORY; + goto error_free; + } + + // Allocate the write buffer. + hdlc->wbuf = malloc (osize); + if (hdlc->wbuf == NULL) { + ERROR (context, "Failed to allocate memory."); + status = DC_STATUS_NOMEMORY; + goto error_free_rbuf; + } + + hdlc->context = context; + hdlc->iostream = base; + hdlc->rbuf_size = isize; + hdlc->rbuf_offset = 0; + hdlc->rbuf_available = 0; + hdlc->wbuf_size = osize; + hdlc->wbuf_offset = 0; + + *out = (dc_iostream_t *) hdlc; + + return DC_STATUS_SUCCESS; + +error_free_rbuf: + free (hdlc->rbuf); +error_free: + dc_iostream_deallocate ((dc_iostream_t *) hdlc); +error_exit: + return status; +} + +static dc_status_t +dc_hdlc_set_timeout (dc_iostream_t *abstract, int timeout) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_set_timeout (hdlc->iostream, timeout); +} + +static dc_status_t +dc_hdlc_set_break (dc_iostream_t *abstract, unsigned int value) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_set_break (hdlc->iostream, value); +} + +static dc_status_t +dc_hdlc_set_dtr (dc_iostream_t *abstract, unsigned int value) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_set_dtr (hdlc->iostream, value); +} + +static dc_status_t +dc_hdlc_set_rts (dc_iostream_t *abstract, unsigned int value) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_set_rts (hdlc->iostream, value); +} + +static dc_status_t +dc_hdlc_get_lines (dc_iostream_t *abstract, unsigned int *value) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_get_lines (hdlc->iostream, value); +} + +static dc_status_t +dc_hdlc_configure (dc_iostream_t *abstract, unsigned int baudrate, unsigned int databits, dc_parity_t parity, dc_stopbits_t stopbits, dc_flowcontrol_t flowcontrol) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_configure (hdlc->iostream, baudrate, databits, parity, stopbits, flowcontrol); +} + +static dc_status_t +dc_hdlc_poll (dc_iostream_t *abstract, int timeout) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + if (hdlc->rbuf_available) { + return DC_STATUS_SUCCESS; + } + + return dc_iostream_poll (hdlc->iostream, timeout); +} + +static dc_status_t +dc_hdlc_read (dc_iostream_t *abstract, void *data, size_t size, size_t *actual) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + size_t nbytes = 0; + + unsigned int initialized = 0; + unsigned int escaped = 0; + + while (1) { + if (hdlc->rbuf_available == 0) { + // Read a packet into the cache. + size_t len = 0; + status = dc_iostream_read (hdlc->iostream, hdlc->rbuf, hdlc->rbuf_size, &len); + if (status != DC_STATUS_SUCCESS) { + goto out; + } + + hdlc->rbuf_available = len; + hdlc->rbuf_offset = 0; + } + + while (hdlc->rbuf_available) { + unsigned char c = hdlc->rbuf[hdlc->rbuf_offset]; + hdlc->rbuf_offset++; + hdlc->rbuf_available--; + + if (c == END) { + if (escaped) { + ERROR (hdlc->context, "HDLC frame escaped the special character %02x.", c); + status = DC_STATUS_IO; + goto out; + } + + if (initialized) { + goto out; + } + + initialized = 1; + continue; + } + + if (!initialized) { + continue; + } + + if (c == ESC) { + if (escaped) { + ERROR (hdlc->context, "HDLC frame escaped the special character %02x.", c); + status = DC_STATUS_IO; + goto out; + } + escaped = 1; + continue; + } + + if (escaped) { + c ^= ESC_BIT; + escaped = 0; + } + + if (nbytes < size) + ((unsigned char *)data)[nbytes] = c; + nbytes++; + } + } + +out: + if (nbytes > size) { + ERROR (hdlc->context, "HDLC frame is too large (" DC_PRINTF_SIZE " " DC_PRINTF_SIZE ").", nbytes, size); + dc_status_set_error (&status, DC_STATUS_IO); + nbytes = size; + } + + if (actual) + *actual = nbytes; + + return status; +} + +static dc_status_t +dc_hdlc_write (dc_iostream_t *abstract, const void *data, size_t size, size_t *actual) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + size_t nbytes = 0; + + // Clear the buffer. + hdlc->wbuf_offset = 0; + + // Start of the packet. + hdlc->wbuf[hdlc->wbuf_offset++] = END; + + // Flush the buffer if necessary. + if (hdlc->wbuf_offset >= hdlc->wbuf_size) { + status = dc_iostream_write (hdlc->iostream, hdlc->wbuf, hdlc->wbuf_offset, NULL); + if (status != DC_STATUS_SUCCESS) { + goto out; + } + + hdlc->wbuf_offset = 0; + } + + while (nbytes < size) { + unsigned char c = ((const unsigned char *) data)[nbytes]; + + if (c == END || c == ESC) { + // Append the escape character. + hdlc->wbuf[hdlc->wbuf_offset++] = ESC; + + // Flush the buffer if necessary. + if (hdlc->wbuf_offset >= hdlc->wbuf_size) { + status = dc_iostream_write (hdlc->iostream, hdlc->wbuf, hdlc->wbuf_offset, NULL); + if (status != DC_STATUS_SUCCESS) { + goto out; + } + + hdlc->wbuf_offset = 0; + } + + // Escape the character. + c ^= ESC_BIT; + } + + // Append the character. + hdlc->wbuf[hdlc->wbuf_offset++] = c; + + // Flush the buffer if necessary. + if (hdlc->wbuf_offset >= hdlc->wbuf_size) { + status = dc_iostream_write (hdlc->iostream, hdlc->wbuf, hdlc->wbuf_offset, NULL); + if (status != DC_STATUS_SUCCESS) { + goto out; + } + + hdlc->wbuf_offset = 0; + } + + nbytes++; + } + + // End of the packet. + hdlc->wbuf[hdlc->wbuf_offset++] = END; + + // Flush the buffer. + status = dc_iostream_write (hdlc->iostream, hdlc->wbuf, hdlc->wbuf_offset, NULL); + if (status != DC_STATUS_SUCCESS) { + goto out; + } + + hdlc->wbuf_offset = 0; + +out: + if (actual) + *actual = nbytes; + + return status; +} + +static dc_status_t +dc_hdlc_ioctl (dc_iostream_t *abstract, unsigned int request, void *data, size_t size) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_ioctl (hdlc->iostream, request, data, size); +} + +static dc_status_t +dc_hdlc_flush (dc_iostream_t *abstract) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_flush (hdlc->iostream); +} + +static dc_status_t +dc_hdlc_purge (dc_iostream_t *abstract, dc_direction_t direction) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + if (direction & DC_DIRECTION_INPUT) { + hdlc->rbuf_available = 0; + hdlc->rbuf_offset = 0; + } + + return dc_iostream_purge (hdlc->iostream, direction); +} + +static dc_status_t +dc_hdlc_sleep (dc_iostream_t *abstract, unsigned int milliseconds) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + return dc_iostream_sleep (hdlc->iostream, milliseconds); +} + +static dc_status_t +dc_hdlc_close (dc_iostream_t *abstract) +{ + dc_hdlc_t *hdlc = (dc_hdlc_t *) abstract; + + free (hdlc->wbuf); + free (hdlc->rbuf); + + return DC_STATUS_SUCCESS; +} diff --git a/src/hdlc.h b/src/hdlc.h new file mode 100644 index 0000000..aedd9b5 --- /dev/null +++ b/src/hdlc.h @@ -0,0 +1,50 @@ +/* + * libdivecomputer + * + * Copyright (C) 2023 Jef Driesen + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301 USA + */ + +#ifndef DC_HDLC_H +#define DC_HDLC_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/** + * Create a HDLC I/O stream layered on top of another base I/O stream. + * + * @param[out] iostream A location to store the HDLC I/O stream. + * @param[in] context A valid context. + * @param[in] base A valid I/O stream. + * @param[in] isize The input packet size in bytes. + * @param[in] osize The output packet size in bytes. + * @returns #DC_STATUS_SUCCESS on success, or another #dc_status_t code + * on failure. + */ +dc_status_t +dc_hdlc_open (dc_iostream_t **iostream, dc_context_t *context, dc_iostream_t *base, size_t isize, size_t osize); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ +#endif /* DC_HDLC_H */