drivers/platform/surface/aggregator/ssh_parser.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * SSH message parser.
   4  *
   5  * Copyright (C) 2019-2021 Maximilian Luz <luzmaximilian@gmail.com>
   6  */
   7
   8 #include <asm/unaligned.h>
   9 #include <linux/compiler.h>
  10 #include <linux/device.h>
  11 #include <linux/types.h>
  12
  13 #include <linux/surface_aggregator/serial_hub.h>
  14 #include "ssh_parser.h"
  15
  16 /**
  17  * sshp_validate_crc() - Validate a CRC in raw message data.
  18  * @src: The span of data over which the CRC should be computed.
  19  * @crc: The pointer to the expected u16 CRC value.
  20  *
  21  * Computes the CRC of the provided data span (@src), compares it to the CRC
  22  * stored at the given address (@crc), and returns the result of this
  23  * comparison, i.e. %true if equal. This function is intended to run on raw
  24  * input/message data.
  25  *
  26  * Return: Returns %true if the computed CRC matches the stored CRC, %false
  27  * otherwise.
  28  */
  29 static bool sshp_validate_crc(const struct ssam_span *src, const u8 *crc)
  30 {
  31         u16 actual = ssh_crc(src->ptr, src->len);
  32         u16 expected = get_unaligned_le16(crc);
  33
  34         return actual == expected;
  35 }
  36
  37 /**
  38  * sshp_starts_with_syn() - Check if the given data starts with SSH SYN bytes.
  39  * @src: The data span to check the start of.
  40  */
  41 static bool sshp_starts_with_syn(const struct ssam_span *src)
  42 {
  43         return src->len >= 2 && get_unaligned_le16(src->ptr) == SSH_MSG_SYN;
  44 }
  45
  46 /**
  47  * sshp_find_syn() - Find SSH SYN bytes in the given data span.
  48  * @src: The data span to search in.
  49  * @rem: The span (output) indicating the remaining data, starting with SSH
  50  *       SYN bytes, if found.
  51  *
  52  * Search for SSH SYN bytes in the given source span. If found, set the @rem
  53  * span to the remaining data, starting with the first SYN bytes and capped by
  54  * the source span length, and return %true. This function does not copy any
  55  * data, but rather only sets pointers to the respective start addresses and
  56  * length values.
  57  *
  58  * If no SSH SYN bytes could be found, set the @rem span to the zero-length
  59  * span at the end of the source span and return %false.
  60  *
  61  * If partial SSH SYN bytes could be found at the end of the source span, set
  62  * the @rem span to cover these partial SYN bytes, capped by the end of the
  63  * source span, and return %false. This function should then be re-run once
  64  * more data is available.
  65  *
  66  * Return: Returns %true if a complete SSH SYN sequence could be found,
  67  * %false otherwise.
  68  */
  69 bool sshp_find_syn(const struct ssam_span *src, struct ssam_span *rem)
  70 {
  71         size_t i;
  72
  73         for (i = 0; i < src->len - 1; i++) {
  74                 if (likely(get_unaligned_le16(src->ptr + i) == SSH_MSG_SYN)) {
  75                         rem->ptr = src->ptr + i;
  76                         rem->len = src->len - i;
  77                         return true;
  78                 }
  79         }
  80
  81         if (unlikely(src->ptr[src->len - 1] == (SSH_MSG_SYN & 0xff))) {
  82                 rem->ptr = src->ptr + src->len - 1;
  83                 rem->len = 1;
  84                 return false;
  85         }
  86
  87         rem->ptr = src->ptr + src->len;
  88         rem->len = 0;
  89         return false;
  90 }
  91
  92 /**
  93  * sshp_parse_frame() - Parse SSH frame.
  94  * @dev: The device used for logging.
  95  * @source: The source to parse from.
  96  * @frame: The parsed frame (output).
  97  * @payload: The parsed payload (output).
  98  * @maxlen: The maximum supported message length.
  99  *
 100  * Parses and validates a SSH frame, including its payload, from the given
 101  * source. Sets the provided @frame pointer to the start of the frame and
 102  * writes the limits of the frame payload to the provided @payload span
 103  * pointer.
 104  *
 105  * This function does not copy any data, but rather only validates the message
 106  * data and sets pointers (and length values) to indicate the respective parts.
 107  *
 108  * If no complete SSH frame could be found, the frame pointer will be set to
 109  * the %NULL pointer and the payload span will be set to the null span (start
 110  * pointer %NULL, size zero).
 111  *
 112  * Return: Returns zero on success or if the frame is incomplete, %-ENOMSG if
 113  * the start of the message is invalid, %-EBADMSG if any (frame-header or
 114  * payload) CRC is invalid, or %-EMSGSIZE if the SSH message is bigger than
 115  * the maximum message length specified in the @maxlen parameter.
 116  */
 117 int sshp_parse_frame(const struct device *dev, const struct ssam_span *source,
 118                      struct ssh_frame **frame, struct ssam_span *payload,
 119                      size_t maxlen)
 120 {
 121         struct ssam_span sf;
 122         struct ssam_span sp;
 123
 124         /* Initialize output. */
 125         *frame = NULL;
 126         payload->ptr = NULL;
 127         payload->len = 0;
 128
 129         if (!sshp_starts_with_syn(source)) {
 130                 dev_warn(dev, "rx: parser: invalid start of frame\n");
 131                 return -ENOMSG;
 132         }
 133
 134         /* Check for minimum packet length. */
 135         if (unlikely(source->len < SSH_MESSAGE_LENGTH(0))) {
 136                 dev_dbg(dev, "rx: parser: not enough data for frame\n");
 137                 return 0;
 138         }
 139
 140         /* Pin down frame. */
 141         sf.ptr = source->ptr + sizeof(u16);
 142         sf.len = sizeof(struct ssh_frame);
 143
 144         /* Validate frame CRC. */
 145         if (unlikely(!sshp_validate_crc(&sf, sf.ptr + sf.len))) {
 146                 dev_warn(dev, "rx: parser: invalid frame CRC\n");
 147                 return -EBADMSG;
 148         }
 149
 150         /* Ensure packet does not exceed maximum length. */
 151         sp.len = get_unaligned_le16(&((struct ssh_frame *)sf.ptr)->len);
 152         if (unlikely(SSH_MESSAGE_LENGTH(sp.len) > maxlen)) {
 153                 dev_warn(dev, "rx: parser: frame too large: %llu bytes\n",
 154                          SSH_MESSAGE_LENGTH(sp.len));
 155                 return -EMSGSIZE;
 156         }
 157
 158         /* Pin down payload. */
 159         sp.ptr = sf.ptr + sf.len + sizeof(u16);
 160
 161         /* Check for frame + payload length. */
 162         if (source->len < SSH_MESSAGE_LENGTH(sp.len)) {
 163                 dev_dbg(dev, "rx: parser: not enough data for payload\n");
 164                 return 0;
 165         }
 166
 167         /* Validate payload CRC. */
 168         if (unlikely(!sshp_validate_crc(&sp, sp.ptr + sp.len))) {
 169                 dev_warn(dev, "rx: parser: invalid payload CRC\n");
 170                 return -EBADMSG;
 171         }
 172
 173         *frame = (struct ssh_frame *)sf.ptr;
 174         *payload = sp;
 175
 176         dev_dbg(dev, "rx: parser: valid frame found (type: %#04x, len: %u)\n",
 177                 (*frame)->type, (*frame)->len);
 178
 179         return 0;
 180 }
 181
 182 /**
 183  * sshp_parse_command() - Parse SSH command frame payload.
 184  * @dev: The device used for logging.
 185  * @source: The source to parse from.
 186  * @command: The parsed command (output).
 187  * @command_data: The parsed command data/payload (output).
 188  *
 189  * Parses and validates a SSH command frame payload. Sets the @command pointer
 190  * to the command header and the @command_data span to the command data (i.e.
 191  * payload of the command). This will result in a zero-length span if the
 192  * command does not have any associated data/payload. This function does not
 193  * check the frame-payload-type field, which should be checked by the caller
 194  * before calling this function.
 195  *
 196  * The @source parameter should be the complete frame payload, e.g. returned
 197  * by the sshp_parse_frame() command.
 198  *
 199  * This function does not copy any data, but rather only validates the frame
 200  * payload data and sets pointers (and length values) to indicate the
 201  * respective parts.
 202  *
 203  * Return: Returns zero on success or %-ENOMSG if @source does not represent a
 204  * valid command-type frame payload, i.e. is too short.
 205  */
 206 int sshp_parse_command(const struct device *dev, const struct ssam_span *source,
 207                        struct ssh_command **command,
 208                        struct ssam_span *command_data)
 209 {
 210         /* Check for minimum length. */
 211         if (unlikely(source->len < sizeof(struct ssh_command))) {
 212                 *command = NULL;
 213                 command_data->ptr = NULL;
 214                 command_data->len = 0;
 215
 216                 dev_err(dev, "rx: parser: command payload is too short\n");
 217                 return -ENOMSG;
 218         }
 219
 220         *command = (struct ssh_command *)source->ptr;
 221         command_data->ptr = source->ptr + sizeof(struct ssh_command);
 222         command_data->len = source->len - sizeof(struct ssh_command);
 223
 224         dev_dbg(dev, "rx: parser: valid command found (tc: %#04x, cid: %#04x)\n",
 225                 (*command)->tc, (*command)->cid);
 226
 227         return 0;
 228 }