linux/drivers/platform/surface/aggregator/ssh_parser.c
<<
>>
Prefs
   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 */
  29static 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 */
  41static 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 */
  69bool 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 */
 117int 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 */
 206int 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}
 229