229 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/*3 * SSH message parser.4 *5 * Copyright (C) 2019-2022 Maximilian Luz <luzmaximilian@gmail.com>6 */7 8#include <linux/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 CRC22 * stored at the given address (@crc), and returns the result of this23 * comparison, i.e. %true if equal. This function is intended to run on raw24 * input/message data.25 *26 * Return: Returns %true if the computed CRC matches the stored CRC, %false27 * 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 SSH50 * SYN bytes, if found.51 *52 * Search for SSH SYN bytes in the given source span. If found, set the @rem53 * span to the remaining data, starting with the first SYN bytes and capped by54 * the source span length, and return %true. This function does not copy any55 * data, but rather only sets pointers to the respective start addresses and56 * length values.57 *58 * If no SSH SYN bytes could be found, set the @rem span to the zero-length59 * 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, set62 * the @rem span to cover these partial SYN bytes, capped by the end of the63 * source span, and return %false. This function should then be re-run once64 * 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 given101 * source. Sets the provided @frame pointer to the start of the frame and102 * writes the limits of the frame payload to the provided @payload span103 * pointer.104 *105 * This function does not copy any data, but rather only validates the message106 * 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 to109 * the %NULL pointer and the payload span will be set to the null span (start110 * pointer %NULL, size zero).111 *112 * Return: Returns zero on success or if the frame is incomplete, %-ENOMSG if113 * the start of the message is invalid, %-EBADMSG if any (frame-header or114 * payload) CRC is invalid, or %-EMSGSIZE if the SSH message is bigger than115 * 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 pointer190 * 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 the192 * command does not have any associated data/payload. This function does not193 * check the frame-payload-type field, which should be checked by the caller194 * before calling this function.195 *196 * The @source parameter should be the complete frame payload, e.g. returned197 * by the sshp_parse_frame() command.198 *199 * This function does not copy any data, but rather only validates the frame200 * payload data and sets pointers (and length values) to indicate the201 * respective parts.202 *203 * Return: Returns zero on success or %-ENOMSG if @source does not represent a204 * 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