surface/aggregator/ssh_parser.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * SSH message parser.
0004  *
0005  * Copyright (C) 2019-2022 Maximilian Luz <luzmaximilian@gmail.com>
0006  */
0007
0008 #include <asm/unaligned.h>
0009 #include <linux/compiler.h>
0010 #include <linux/device.h>
0011 #include <linux/types.h>
0012
0013 #include <linux/surface_aggregator/serial_hub.h>
0014 #include "ssh_parser.h"
0015
0016 /**
0017  * sshp_validate_crc() - Validate a CRC in raw message data.
0018  * @src: The span of data over which the CRC should be computed.
0019  * @crc: The pointer to the expected u16 CRC value.
0020  *
0021  * Computes the CRC of the provided data span (@src), compares it to the CRC
0022  * stored at the given address (@crc), and returns the result of this
0023  * comparison, i.e. %true if equal. This function is intended to run on raw
0024  * input/message data.
0025  *
0026  * Return: Returns %true if the computed CRC matches the stored CRC, %false
0027  * otherwise.
0028  */
0029 static bool sshp_validate_crc(const struct ssam_span *src, const u8 *crc)
0030 {
0031     u16 actual = ssh_crc(src->ptr, src->len);
0032     u16 expected = get_unaligned_le16(crc);
0033
0034     return actual == expected;
0035 }
0036
0037 /**
0038  * sshp_starts_with_syn() - Check if the given data starts with SSH SYN bytes.
0039  * @src: The data span to check the start of.
0040  */
0041 static bool sshp_starts_with_syn(const struct ssam_span *src)
0042 {
0043     return src->len >= 2 && get_unaligned_le16(src->ptr) == SSH_MSG_SYN;
0044 }
0045
0046 /**
0047  * sshp_find_syn() - Find SSH SYN bytes in the given data span.
0048  * @src: The data span to search in.
0049  * @rem: The span (output) indicating the remaining data, starting with SSH
0050  *       SYN bytes, if found.
0051  *
0052  * Search for SSH SYN bytes in the given source span. If found, set the @rem
0053  * span to the remaining data, starting with the first SYN bytes and capped by
0054  * the source span length, and return %true. This function does not copy any
0055  * data, but rather only sets pointers to the respective start addresses and
0056  * length values.
0057  *
0058  * If no SSH SYN bytes could be found, set the @rem span to the zero-length
0059  * span at the end of the source span and return %false.
0060  *
0061  * If partial SSH SYN bytes could be found at the end of the source span, set
0062  * the @rem span to cover these partial SYN bytes, capped by the end of the
0063  * source span, and return %false. This function should then be re-run once
0064  * more data is available.
0065  *
0066  * Return: Returns %true if a complete SSH SYN sequence could be found,
0067  * %false otherwise.
0068  */
0069 bool sshp_find_syn(const struct ssam_span *src, struct ssam_span *rem)
0070 {
0071     size_t i;
0072
0073     for (i = 0; i < src->len - 1; i++) {
0074         if (likely(get_unaligned_le16(src->ptr + i) == SSH_MSG_SYN)) {
0075             rem->ptr = src->ptr + i;
0076             rem->len = src->len - i;
0077             return true;
0078         }
0079     }
0080
0081     if (unlikely(src->ptr[src->len - 1] == (SSH_MSG_SYN & 0xff))) {
0082         rem->ptr = src->ptr + src->len - 1;
0083         rem->len = 1;
0084         return false;
0085     }
0086
0087     rem->ptr = src->ptr + src->len;
0088     rem->len = 0;
0089     return false;
0090 }
0091
0092 /**
0093  * sshp_parse_frame() - Parse SSH frame.
0094  * @dev: The device used for logging.
0095  * @source: The source to parse from.
0096  * @frame: The parsed frame (output).
0097  * @payload: The parsed payload (output).
0098  * @maxlen: The maximum supported message length.
0099  *
0100  * Parses and validates a SSH frame, including its payload, from the given
0101  * source. Sets the provided @frame pointer to the start of the frame and
0102  * writes the limits of the frame payload to the provided @payload span
0103  * pointer.
0104  *
0105  * This function does not copy any data, but rather only validates the message
0106  * data and sets pointers (and length values) to indicate the respective parts.
0107  *
0108  * If no complete SSH frame could be found, the frame pointer will be set to
0109  * the %NULL pointer and the payload span will be set to the null span (start
0110  * pointer %NULL, size zero).
0111  *
0112  * Return: Returns zero on success or if the frame is incomplete, %-ENOMSG if
0113  * the start of the message is invalid, %-EBADMSG if any (frame-header or
0114  * payload) CRC is invalid, or %-EMSGSIZE if the SSH message is bigger than
0115  * the maximum message length specified in the @maxlen parameter.
0116  */
0117 int sshp_parse_frame(const struct device *dev, const struct ssam_span *source,
0118              struct ssh_frame **frame, struct ssam_span *payload,
0119              size_t maxlen)
0120 {
0121     struct ssam_span sf;
0122     struct ssam_span sp;
0123
0124     /* Initialize output. */
0125     *frame = NULL;
0126     payload->ptr = NULL;
0127     payload->len = 0;
0128
0129     if (!sshp_starts_with_syn(source)) {
0130         dev_warn(dev, "rx: parser: invalid start of frame\n");
0131         return -ENOMSG;
0132     }
0133
0134     /* Check for minimum packet length. */
0135     if (unlikely(source->len < SSH_MESSAGE_LENGTH(0))) {
0136         dev_dbg(dev, "rx: parser: not enough data for frame\n");
0137         return 0;
0138     }
0139
0140     /* Pin down frame. */
0141     sf.ptr = source->ptr + sizeof(u16);
0142     sf.len = sizeof(struct ssh_frame);
0143
0144     /* Validate frame CRC. */
0145     if (unlikely(!sshp_validate_crc(&sf, sf.ptr + sf.len))) {
0146         dev_warn(dev, "rx: parser: invalid frame CRC\n");
0147         return -EBADMSG;
0148     }
0149
0150     /* Ensure packet does not exceed maximum length. */
0151     sp.len = get_unaligned_le16(&((struct ssh_frame *)sf.ptr)->len);
0152     if (unlikely(SSH_MESSAGE_LENGTH(sp.len) > maxlen)) {
0153         dev_warn(dev, "rx: parser: frame too large: %llu bytes\n",
0154              SSH_MESSAGE_LENGTH(sp.len));
0155         return -EMSGSIZE;
0156     }
0157
0158     /* Pin down payload. */
0159     sp.ptr = sf.ptr + sf.len + sizeof(u16);
0160
0161     /* Check for frame + payload length. */
0162     if (source->len < SSH_MESSAGE_LENGTH(sp.len)) {
0163         dev_dbg(dev, "rx: parser: not enough data for payload\n");
0164         return 0;
0165     }
0166
0167     /* Validate payload CRC. */
0168     if (unlikely(!sshp_validate_crc(&sp, sp.ptr + sp.len))) {
0169         dev_warn(dev, "rx: parser: invalid payload CRC\n");
0170         return -EBADMSG;
0171     }
0172
0173     *frame = (struct ssh_frame *)sf.ptr;
0174     *payload = sp;
0175
0176     dev_dbg(dev, "rx: parser: valid frame found (type: %#04x, len: %u)\n",
0177         (*frame)->type, (*frame)->len);
0178
0179     return 0;
0180 }
0181
0182 /**
0183  * sshp_parse_command() - Parse SSH command frame payload.
0184  * @dev: The device used for logging.
0185  * @source: The source to parse from.
0186  * @command: The parsed command (output).
0187  * @command_data: The parsed command data/payload (output).
0188  *
0189  * Parses and validates a SSH command frame payload. Sets the @command pointer
0190  * to the command header and the @command_data span to the command data (i.e.
0191  * payload of the command). This will result in a zero-length span if the
0192  * command does not have any associated data/payload. This function does not
0193  * check the frame-payload-type field, which should be checked by the caller
0194  * before calling this function.
0195  *
0196  * The @source parameter should be the complete frame payload, e.g. returned
0197  * by the sshp_parse_frame() command.
0198  *
0199  * This function does not copy any data, but rather only validates the frame
0200  * payload data and sets pointers (and length values) to indicate the
0201  * respective parts.
0202  *
0203  * Return: Returns zero on success or %-ENOMSG if @source does not represent a
0204  * valid command-type frame payload, i.e. is too short.
0205  */
0206 int sshp_parse_command(const struct device *dev, const struct ssam_span *source,
0207                struct ssh_command **command,
0208                struct ssam_span *command_data)
0209 {
0210     /* Check for minimum length. */
0211     if (unlikely(source->len < sizeof(struct ssh_command))) {
0212         *command = NULL;
0213         command_data->ptr = NULL;
0214         command_data->len = 0;
0215
0216         dev_err(dev, "rx: parser: command payload is too short\n");
0217         return -ENOMSG;
0218     }
0219
0220     *command = (struct ssh_command *)source->ptr;
0221     command_data->ptr = source->ptr + sizeof(struct ssh_command);
0222     command_data->len = source->len - sizeof(struct ssh_command);
0223
0224     dev_dbg(dev, "rx: parser: valid command found (tc: %#04x, cid: %#04x)\n",
0225         (*command)->tc, (*command)->cid);
0226
0227     return 0;
0228 }