462 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * MIPI SyS-T framing protocol for STM devices.4 * Copyright (c) 2018, Intel Corporation.5 */6 7#include <linux/configfs.h>8#include <linux/module.h>9#include <linux/device.h>10#include <linux/slab.h>11#include <linux/uuid.h>12#include <linux/stm.h>13#include "stm.h"14 15enum sys_t_message_type {16 MIPI_SYST_TYPE_BUILD = 0,17 MIPI_SYST_TYPE_SHORT32,18 MIPI_SYST_TYPE_STRING,19 MIPI_SYST_TYPE_CATALOG,20 MIPI_SYST_TYPE_RAW = 6,21 MIPI_SYST_TYPE_SHORT64,22 MIPI_SYST_TYPE_CLOCK,23 MIPI_SYST_TYPE_SBD,24};25 26enum sys_t_message_severity {27 MIPI_SYST_SEVERITY_MAX = 0,28 MIPI_SYST_SEVERITY_FATAL,29 MIPI_SYST_SEVERITY_ERROR,30 MIPI_SYST_SEVERITY_WARNING,31 MIPI_SYST_SEVERITY_INFO,32 MIPI_SYST_SEVERITY_USER1,33 MIPI_SYST_SEVERITY_USER2,34 MIPI_SYST_SEVERITY_DEBUG,35};36 37enum sys_t_message_build_subtype {38 MIPI_SYST_BUILD_ID_COMPACT32 = 0,39 MIPI_SYST_BUILD_ID_COMPACT64,40 MIPI_SYST_BUILD_ID_LONG,41};42 43enum sys_t_message_clock_subtype {44 MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,45};46 47enum sys_t_message_string_subtype {48 MIPI_SYST_STRING_GENERIC = 1,49 MIPI_SYST_STRING_FUNCTIONENTER,50 MIPI_SYST_STRING_FUNCTIONEXIT,51 MIPI_SYST_STRING_INVALIDPARAM = 5,52 MIPI_SYST_STRING_ASSERT = 7,53 MIPI_SYST_STRING_PRINTF_32 = 11,54 MIPI_SYST_STRING_PRINTF_64 = 12,55};56 57/**58 * enum sys_t_message_sbd_subtype - SyS-T SBD message subtypes59 * @MIPI_SYST_SBD_ID32: SBD message with 32-bit message ID60 * @MIPI_SYST_SBD_ID64: SBD message with 64-bit message ID61 *62 * Structured Binary Data messages can send information of arbitrary length,63 * together with ID's that describe BLOB's content and layout.64 */65enum sys_t_message_sbd_subtype {66 MIPI_SYST_SBD_ID32 = 0,67 MIPI_SYST_SBD_ID64 = 1,68};69 70#define MIPI_SYST_TYPE(t) ((u32)(MIPI_SYST_TYPE_ ## t))71#define MIPI_SYST_SEVERITY(s) ((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)72#define MIPI_SYST_OPT_LOC BIT(8)73#define MIPI_SYST_OPT_LEN BIT(9)74#define MIPI_SYST_OPT_CHK BIT(10)75#define MIPI_SYST_OPT_TS BIT(11)76#define MIPI_SYST_UNIT(u) ((u32)(u) << 12)77#define MIPI_SYST_ORIGIN(o) ((u32)(o) << 16)78#define MIPI_SYST_OPT_GUID BIT(23)79#define MIPI_SYST_SUBTYPE(s) ((u32)(MIPI_SYST_ ## s) << 24)80#define MIPI_SYST_UNITLARGE(u) (MIPI_SYST_UNIT(u & 0xf) | \81 MIPI_SYST_ORIGIN(u >> 4))82#define MIPI_SYST_TYPES(t, s) (MIPI_SYST_TYPE(t) | \83 MIPI_SYST_SUBTYPE(t ## _ ## s))84 85#define DATA_HEADER (MIPI_SYST_TYPES(STRING, GENERIC) | \86 MIPI_SYST_SEVERITY(INFO) | \87 MIPI_SYST_OPT_GUID)88 89#define CLOCK_SYNC_HEADER (MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC) | \90 MIPI_SYST_SEVERITY(MAX))91 92/*93 * SyS-T and ftrace headers are compatible to an extent that ftrace event ID94 * and args can be treated as SyS-T SBD message with 64-bit ID and arguments95 * BLOB right behind the header without modification. Bits [16:63] coming96 * together with message ID from ftrace aren't used by SBD and must be zeroed.97 *98 * 0 15 16 23 24 31 32 39 40 6399 * ftrace: <event_id> <flags> <preempt> <-pid-> <----> <args>100 * SBD: <------- msg_id ------------------------------> <BLOB>101 */102#define SBD_HEADER (MIPI_SYST_TYPES(SBD, ID64) | \103 MIPI_SYST_SEVERITY(INFO) | \104 MIPI_SYST_OPT_GUID)105 106struct sys_t_policy_node {107 uuid_t uuid;108 bool do_len;109 unsigned long ts_interval;110 unsigned long clocksync_interval;111};112 113struct sys_t_output {114 struct sys_t_policy_node node;115 unsigned long ts_jiffies;116 unsigned long clocksync_jiffies;117};118 119static void sys_t_policy_node_init(void *priv)120{121 struct sys_t_policy_node *pn = priv;122 123 uuid_gen(&pn->uuid);124}125 126static int sys_t_output_open(void *priv, struct stm_output *output)127{128 struct sys_t_policy_node *pn = priv;129 struct sys_t_output *opriv;130 131 opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);132 if (!opriv)133 return -ENOMEM;134 135 memcpy(&opriv->node, pn, sizeof(opriv->node));136 output->pdrv_private = opriv;137 138 return 0;139}140 141static void sys_t_output_close(struct stm_output *output)142{143 kfree(output->pdrv_private);144}145 146static ssize_t sys_t_policy_uuid_show(struct config_item *item,147 char *page)148{149 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);150 151 return sprintf(page, "%pU\n", &pn->uuid);152}153 154static ssize_t155sys_t_policy_uuid_store(struct config_item *item, const char *page,156 size_t count)157{158 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;159 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);160 int ret;161 162 mutex_lock(mutexp);163 ret = uuid_parse(page, &pn->uuid);164 mutex_unlock(mutexp);165 166 return ret < 0 ? ret : count;167}168 169CONFIGFS_ATTR(sys_t_policy_, uuid);170 171static ssize_t sys_t_policy_do_len_show(struct config_item *item,172 char *page)173{174 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);175 176 return sprintf(page, "%d\n", pn->do_len);177}178 179static ssize_t180sys_t_policy_do_len_store(struct config_item *item, const char *page,181 size_t count)182{183 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;184 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);185 int ret;186 187 mutex_lock(mutexp);188 ret = kstrtobool(page, &pn->do_len);189 mutex_unlock(mutexp);190 191 return ret ? ret : count;192}193 194CONFIGFS_ATTR(sys_t_policy_, do_len);195 196static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,197 char *page)198{199 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);200 201 return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));202}203 204static ssize_t205sys_t_policy_ts_interval_store(struct config_item *item, const char *page,206 size_t count)207{208 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;209 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);210 unsigned int ms;211 int ret;212 213 mutex_lock(mutexp);214 ret = kstrtouint(page, 10, &ms);215 mutex_unlock(mutexp);216 217 if (!ret) {218 pn->ts_interval = msecs_to_jiffies(ms);219 return count;220 }221 222 return ret;223}224 225CONFIGFS_ATTR(sys_t_policy_, ts_interval);226 227static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,228 char *page)229{230 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);231 232 return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));233}234 235static ssize_t236sys_t_policy_clocksync_interval_store(struct config_item *item,237 const char *page, size_t count)238{239 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;240 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);241 unsigned int ms;242 int ret;243 244 mutex_lock(mutexp);245 ret = kstrtouint(page, 10, &ms);246 mutex_unlock(mutexp);247 248 if (!ret) {249 pn->clocksync_interval = msecs_to_jiffies(ms);250 return count;251 }252 253 return ret;254}255 256CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);257 258static struct configfs_attribute *sys_t_policy_attrs[] = {259 &sys_t_policy_attr_uuid,260 &sys_t_policy_attr_do_len,261 &sys_t_policy_attr_ts_interval,262 &sys_t_policy_attr_clocksync_interval,263 NULL,264};265 266static inline bool sys_t_need_ts(struct sys_t_output *op)267{268 if (op->node.ts_interval &&269 time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {270 op->ts_jiffies = jiffies;271 272 return true;273 }274 275 return false;276}277 278static bool sys_t_need_clock_sync(struct sys_t_output *op)279{280 if (op->node.clocksync_interval &&281 time_after(jiffies,282 op->clocksync_jiffies + op->node.clocksync_interval)) {283 op->clocksync_jiffies = jiffies;284 285 return true;286 }287 288 return false;289}290 291static ssize_t292sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)293{294 u32 header = CLOCK_SYNC_HEADER;295 const unsigned char nil = 0;296 u64 payload[2]; /* Clock value and frequency */297 ssize_t sz;298 299 sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,300 4, (u8 *)&header);301 if (sz <= 0)302 return sz;303 304 payload[0] = ktime_get_real_ns();305 payload[1] = NSEC_PER_SEC;306 sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));307 if (sz <= 0)308 return sz;309 310 data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);311 312 return sizeof(header) + sizeof(payload);313}314 315static inline u32 sys_t_header(struct stm_source_data *source)316{317 if (source && source->type == STM_FTRACE)318 return SBD_HEADER;319 return DATA_HEADER;320}321 322static ssize_t sys_t_write_data(struct stm_data *data,323 struct stm_source_data *source,324 unsigned int master, unsigned int channel,325 bool ts_first, const void *buf, size_t count)326{327 ssize_t sz;328 const unsigned char nil = 0;329 330 /*331 * Ftrace is zero-copy compatible with SyS-T SBD, but requires332 * special handling of first 64 bits. Trim and send them separately333 * to avoid damage on original ftrace buffer.334 */335 if (source && source->type == STM_FTRACE) {336 u64 compat_ftrace_header;337 ssize_t header_sz;338 ssize_t buf_sz;339 340 if (count < sizeof(compat_ftrace_header))341 return -EINVAL;342 343 /* SBD only makes use of low 16 bits (event ID) from ftrace event */344 compat_ftrace_header = *(u64 *)buf & 0xffff;345 header_sz = stm_data_write(data, master, channel, false,346 &compat_ftrace_header,347 sizeof(compat_ftrace_header));348 if (header_sz != sizeof(compat_ftrace_header))349 return header_sz;350 351 buf_sz = stm_data_write(data, master, channel, false,352 buf + header_sz, count - header_sz);353 if (buf_sz != count - header_sz)354 return buf_sz;355 sz = header_sz + buf_sz;356 } else {357 sz = stm_data_write(data, master, channel, false, buf, count);358 }359 360 if (sz <= 0)361 return sz;362 363 data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);364 365 return sz;366}367 368static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,369 unsigned int chan, const char *buf, size_t count,370 struct stm_source_data *source)371{372 struct sys_t_output *op = output->pdrv_private;373 unsigned int c = output->channel + chan;374 unsigned int m = output->master;375 u32 header = sys_t_header(source);376 u8 uuid[UUID_SIZE];377 ssize_t sz;378 379 /* We require an existing policy node to proceed */380 if (!op)381 return -EINVAL;382 383 if (sys_t_need_clock_sync(op)) {384 sz = sys_t_clock_sync(data, m, c);385 if (sz <= 0)386 return sz;387 }388 389 if (op->node.do_len)390 header |= MIPI_SYST_OPT_LEN;391 if (sys_t_need_ts(op))392 header |= MIPI_SYST_OPT_TS;393 394 /*395 * STP framing rules for SyS-T frames:396 * * the first packet of the SyS-T frame is timestamped;397 * * the last packet is a FLAG.398 */399 /* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */400 /* HEADER */401 sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,402 4, (u8 *)&header);403 if (sz <= 0)404 return sz;405 406 /* GUID */407 export_uuid(uuid, &op->node.uuid);408 sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid));409 if (sz <= 0)410 return sz;411 412 /* [LENGTH] */413 if (op->node.do_len) {414 u16 length = count;415 416 sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,417 (u8 *)&length);418 if (sz <= 0)419 return sz;420 }421 422 /* [TIMESTAMP] */423 if (header & MIPI_SYST_OPT_TS) {424 u64 ts = ktime_get_real_ns();425 426 sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));427 if (sz <= 0)428 return sz;429 }430 431 /* DATA */432 return sys_t_write_data(data, source, m, c, false, buf, count);433}434 435static const struct stm_protocol_driver sys_t_pdrv = {436 .owner = THIS_MODULE,437 .name = "p_sys-t",438 .priv_sz = sizeof(struct sys_t_policy_node),439 .write = sys_t_write,440 .policy_attr = sys_t_policy_attrs,441 .policy_node_init = sys_t_policy_node_init,442 .output_open = sys_t_output_open,443 .output_close = sys_t_output_close,444};445 446static int sys_t_stm_init(void)447{448 return stm_register_protocol(&sys_t_pdrv);449}450 451static void sys_t_stm_exit(void)452{453 stm_unregister_protocol(&sys_t_pdrv);454}455 456module_init(sys_t_stm_init);457module_exit(sys_t_stm_exit);458 459MODULE_LICENSE("GPL v2");460MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");461MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");462