+/****************************************************************************
+ * protocol.c
+ *
+ *
+ * Copyright 2019 Nathael Pajani <nathael.pajani@ed3l.fr>
+ *
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+
+/* Host side implementation of the communication protocol */
+
+#include <stdint.h>
+#include <string.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <arpa/inet.h>
+#include "protocol.h"
+#include "sock_utils.h"
+
+
+/******************************************************************************/
+/* Handle packet reception, including checksums
+ * 'sum' is used to sum all the received characters, and if the last byte of sum is 0 for each
+ * part (header and data) then the packet is valid.
+ * 'full_size' is the size of the whole packet, including header, updated as soon as the header
+ * is checked and valid
+ *
+ * This function must be called for every received character.
+ * If the character is part of a packet but the packet is being built, then the function returns 0.
+ * When the character is the last one of a valid packet, then the function returns the packet size
+ * and the packet in ser->rx_packet is valid.
+ * If the character is the last one of a packet which has an invalid data checksum, this function
+ * returns -2 and the data is lost.
+ * If the character is not part of a packet it returns -1. The character may then be part of
+ * a debug message (and displayed by the host), or any other kind of communication.
+ * When a set of consecutive characters have been used to build a packet but the packet is
+ * not valid due to header error, then the function returns -3 (checksum error) or -4 (data size
+ * error). The data in ser->rx_packet is the received data but is not valid.
+ * The corresponding data size is always sizeof(struct header).
+ */
+int serial_protocol_decode(char c, struct serial* ser)
+{
+ struct app_header* info = (struct app_header*)(&(ser->packet));
+ int ret = 0;
+
+ /* Do not start reception before receiving the packet start character */
+ if ((ser->rx_ptr == 0) && (c != FIRST_PACKET_CHAR)) {
+ /* This one id not a critical error, but this character is not part of a packet */
+ return -1;
+ }
+
+ /* Store the new byte in the packet */
+ ser->packet[ser->rx_ptr++] = c;
+ ser->sum += c;
+
+ /* Is this packet valid ? (at end of header reception) */
+ if (ser->rx_ptr == HEADER_SIZE) {
+ /* Checksum OK ? */
+ if (ser->sum != 0) {
+ ret = -2;
+ goto next_packet;
+ }
+ /* Start the new checksum for data (if any) */
+ ser->sum = 0;
+ ser->full_size = HEADER_SIZE + info->data_len;
+ /* Make sure the packet will fit in the buffer */
+ if (ser->full_size > MQTT_BUFF_SIZE) {
+ ret = -3;
+ goto next_packet;
+ }
+ }
+
+ /* Did we receive the whole packet ? */
+ if (ser->rx_ptr == ser->full_size) {
+ if (ser->sum != info->data_cksum) {
+ ret = -4;
+ goto next_packet;
+ }
+ ser->pkt_ok_size = ser->full_size;
+ ret = ser->full_size;
+ /* And get ready to receive the next packet */
+ goto next_packet;
+ }
+
+ return 0;
+
+next_packet:
+#ifdef DEBUG
+ printf("BRIDGE: Current rx_ptr: %d, packet full size: %d, ret: %d\n", ser->rx_ptr, ser->full_size, ret);
+ if (ser->rx_ptr >= HEADER_SIZE) {
+ printf("BRIDGE: Pkt: type: %d, addr: %d, seq: %d, data len: %d\n",
+ info->type, ntohs(info->addr), info->seqnum, info->data_len);
+ }
+#endif
+ /* Wether the packet was OK or not doesn't matter, go on for a new one :) */
+ ser->full_size = 0;
+ ser->rx_ptr = 0;
+ ser->sum = 0;
+ return ret;
+}
+
+
+/******************************************************************************/
+/* This function handles sending packets to clients connected over the serial link.
+ * It returns the number of bytes of data sent.
+ */
+int serial_send_packet(struct serial* ser, struct client* target)
+{
+ struct app_header* info;
+ uint32_t len = HEADER_SIZE + target->data_len;
+ uint32_t i = 0, sent = 0;
+ uint8_t sum = 0;
+
+ info = (struct app_header*)target->packet;
+
+ /* Fill header */
+ info->start = '#';
+ info->type = 'M'; /* MQTT packet */
+ info->addr = htons(target->address);
+ info->seqnum = target->seqnum;
+ info->data_len = target->data_len;
+
+ /* Compute data checksum */
+ for (i = HEADER_SIZE; i < len; i++) {
+ sum += target->packet[i];
+ }
+ info->data_cksum = sum;
+
+ /* Compute header checksum */
+ sum = 0;
+ info->header_cksum = 0; /* Erase checksum of previous packet */
+ for (i = 0; i < HEADER_SIZE; i++) {
+ sum += target->packet[i];
+ }
+ info->header_cksum = ((uint8_t)(256 - sum));
+
+ /* And send the packet on the serial link */
+ while (sent < len) {
+ int ret = write(ser->fd, (target->packet + sent), (len - sent));
+ if (ret >= 0) {
+ sent += ret;
+ } else {
+ /* Sending error ... */
+ perror("Serial send error:");
+ /* FIXME : handle / report errors */
+ break;
+ }
+ }
+ /* Packet got sent, get ready for the next one */
+ target->data_len = 0;
+ target->rx_ptr = 0;
+ return sent;
+}
+
+
+/******************************************************************************/
+int handle_server_data(struct serial* ser, struct client* client)
+{
+ return serial_send_packet(ser, client);
+}
+
+
+/******************************************************************************/
+/* This function only decodes the "remaining length" field, in order to pack a complete packet
+ * before sending it to the device.
+ * Aside from getting a valid MQTTT packet size there is no check on validity of either content
+ * or protocol.
+ * On success, returns the number of bytes used from buf.
+ * Returns 0 if there was not enough data to read the size or fill the data buffer.
+ * Returns -1 if length is invalid and the connection should be closed.
+ */
+int server_stream_decode(char* buf, int len, struct client* client)
+{
+ char* mqttbuf = client->packet + HEADER_SIZE;
+ int idx = 0;
+ uint32_t size = 0;
+
+ /* Try to get enough bytes to read the size of the packet */
+ while ((client->data_len == 0) && (idx < len)) {
+ /* Read one byte */
+ mqttbuf[client->rx_ptr++] = buf[idx++];
+ /* Check for size */
+ if ((client->rx_ptr >= 2) && ((mqttbuf[client->rx_ptr - 1] & 0x80) == 0)) {
+ int len_idx = 1;
+ uint32_t length = 0;
+ do {
+ length += (mqttbuf[len_idx] & 0x7F) << (7 * (len_idx - 1));
+ } while (buf[len_idx++] & 0x80);
+ client->data_len = length + len_idx;
+ break;
+ }
+ /* If we already received 5 bytes and were not able to read a packet size, then
+ * there is a protocol error */
+ if (client->rx_ptr >= 5) {
+ return -1;
+ }
+ }
+ /* Some MQTT packet have no data, We are done */
+ if (client->rx_ptr == client->data_len) {
+ return idx;
+ }
+ /* Is there any more data available ? */
+ if ((len - idx) == 0) {
+ return len;
+ }
+ /* Copy buffer to internal buffer, up to the size of one packet */
+ size = len - idx;
+ if ((client->rx_ptr + size) > client->data_len) {
+ size = client->data_len - client->rx_ptr;
+ }
+ memcpy((mqttbuf + client->rx_ptr), (buf + idx), size);
+ client->rx_ptr += size;
+ return idx + size;
+}
+
+
+/******************************************************************************/
+int tcp_send_packet(struct serial* ser, struct client* client)
+{
+ struct app_header* info = NULL;
+ int ret = 0, size = 0;
+ info = (struct app_header*)ser->packet;
+ /* Store sequence number */
+ client->seqnum = info->seqnum;
+ /* And send packet to the server (broker) */
+ size = (ser->pkt_ok_size - HEADER_SIZE);
+ ret = write(client->socket, (ser->packet + HEADER_SIZE), size);
+ if (ret != size) {
+ perror("BRIDGE: Socket write error:");
+ printf("BRIDGE: Transmit error for client(%d) TCP connection", client->address);
+ return ret;
+ }
+ ser->pkt_ok_size = 0;
+ return 0;
+}
+
+
+/******************************************************************************/
+void remove_client(struct internal_data* glob, struct client* client)
+{
+ struct client* ctmp = NULL;
+
+ /* Remove client socket from select list and close socket */
+ FD_CLR(client->socket, &glob->read_fds);
+ close(client->socket);
+ /* Tell the client that the TCP connection got closed ? */
+ /* FIXME */
+ /* Remove client from list of clients */
+ list_del(&client->list);
+ /* Update max_fd */
+ if (list_empty(&glob->clients)) {
+ glob->max_fd = glob->ser.fd + 1;
+ } else {
+ list_for_each_entry(ctmp, &glob->clients, list) {
+ if (ctmp->socket >= glob->max_fd) {
+ glob->max_fd = ctmp->socket + 1;
+ }
+ }
+ }
+ free(client);
+}
+
+/******************************************************************************/
+/* This function is used to match the received packet to an existing client, or create a new
+ * one and open its connection to the server if the client is not in the list.
+ */
+#define MQTT_CONTROL_CONNECT (0x01 << 4)
+int handle_serial_data(struct internal_data* glob)
+{
+ struct client* client = NULL;
+ struct app_header* info = NULL;
+ uint16_t address = 0;
+ int found = 0, ret = 0;
+
+ if (glob == NULL) {
+ return -1;
+ }
+
+ info = (struct app_header*)glob->ser.packet;
+ address = ntohs(info->addr);
+
+ /* Read address from serial input buffer */
+ if (! list_empty(&glob->clients)) {
+ list_for_each_entry(client, &glob->clients, list) {
+ if (client->address == address) {
+ found = 1;
+ break;
+ }
+ }
+ }
+
+ /* Need to create a new client ? */
+ if (found == 0) {
+ client = malloc(sizeof(struct client));
+ memset(client, 0, sizeof(struct client));
+ /* Create TCP client socket */
+ client->socket = socket_tcp_client(glob->ip, glob->port);
+ if (client->socket <= 0) {
+ printf("BRIDGE: Unable to open the TCP socket on port %d\n", glob->port);
+ return -2;
+ }
+ FD_SET(client->socket, &glob->read_fds);
+ if (client->socket >= glob->max_fd) {
+ glob->max_fd = client->socket + 1;
+ }
+ /* Add the client to the list */
+ list_add(&client->list, &glob->clients);
+
+ /* Store address and sequence number */
+ client->address = address;
+ /* This is a new client.
+ * We could check that the packet is an MQTT connect packet and refuse to send it if
+ * it is not, but we would then have to tell the client.
+ * Sending the packet anyway will have the server close the connection, which will
+ * be forwarded to the client, forcing him to re-open the connection using a valid
+ * connect packet.
+ */
+ }
+
+ /* We now have a valid client, */
+ ret = tcp_send_packet(&glob->ser, client);
+ if (ret < 0) {
+ remove_client(glob, client);
+ return -1;
+ }
+ return 0;
+}
+
