drivers/hwtracing/stm/p_sys-t.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   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
  15 enum 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 };
  24
  25 enum sys_t_message_severity {
  26         MIPI_SYST_SEVERITY_MAX  = 0,
  27         MIPI_SYST_SEVERITY_FATAL,
  28         MIPI_SYST_SEVERITY_ERROR,
  29         MIPI_SYST_SEVERITY_WARNING,
  30         MIPI_SYST_SEVERITY_INFO,
  31         MIPI_SYST_SEVERITY_USER1,
  32         MIPI_SYST_SEVERITY_USER2,
  33         MIPI_SYST_SEVERITY_DEBUG,
  34 };
  35
  36 enum sys_t_message_build_subtype {
  37         MIPI_SYST_BUILD_ID_COMPACT32 = 0,
  38         MIPI_SYST_BUILD_ID_COMPACT64,
  39         MIPI_SYST_BUILD_ID_LONG,
  40 };
  41
  42 enum sys_t_message_clock_subtype {
  43         MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
  44 };
  45
  46 enum sys_t_message_string_subtype {
  47         MIPI_SYST_STRING_GENERIC        = 1,
  48         MIPI_SYST_STRING_FUNCTIONENTER,
  49         MIPI_SYST_STRING_FUNCTIONEXIT,
  50         MIPI_SYST_STRING_INVALIDPARAM   = 5,
  51         MIPI_SYST_STRING_ASSERT         = 7,
  52         MIPI_SYST_STRING_PRINTF_32      = 11,
  53         MIPI_SYST_STRING_PRINTF_64      = 12,
  54 };
  55
  56 #define MIPI_SYST_TYPE(t)               ((u32)(MIPI_SYST_TYPE_ ## t))
  57 #define MIPI_SYST_SEVERITY(s)           ((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)
  58 #define MIPI_SYST_OPT_LOC               BIT(8)
  59 #define MIPI_SYST_OPT_LEN               BIT(9)
  60 #define MIPI_SYST_OPT_CHK               BIT(10)
  61 #define MIPI_SYST_OPT_TS                BIT(11)
  62 #define MIPI_SYST_UNIT(u)               ((u32)(u) << 12)
  63 #define MIPI_SYST_ORIGIN(o)             ((u32)(o) << 16)
  64 #define MIPI_SYST_OPT_GUID              BIT(23)
  65 #define MIPI_SYST_SUBTYPE(s)            ((u32)(MIPI_SYST_ ## s) << 24)
  66 #define MIPI_SYST_UNITLARGE(u)          (MIPI_SYST_UNIT(u & 0xf) | \
  67                                          MIPI_SYST_ORIGIN(u >> 4))
  68 #define MIPI_SYST_TYPES(t, s)           (MIPI_SYST_TYPE(t) | \
  69                                          MIPI_SYST_SUBTYPE(t ## _ ## s))
  70
  71 #define DATA_HEADER     (MIPI_SYST_TYPES(STRING, GENERIC)       | \
  72                          MIPI_SYST_SEVERITY(INFO)               | \
  73                          MIPI_SYST_OPT_GUID)
  74
  75 #define CLOCK_SYNC_HEADER       (MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC) | \
  76                                  MIPI_SYST_SEVERITY(MAX))
  77
  78 struct sys_t_policy_node {
  79         uuid_t          uuid;
  80         bool            do_len;
  81         unsigned long   ts_interval;
  82         unsigned long   clocksync_interval;
  83 };
  84
  85 struct sys_t_output {
  86         struct sys_t_policy_node        node;
  87         unsigned long   ts_jiffies;
  88         unsigned long   clocksync_jiffies;
  89 };
  90
  91 static void sys_t_policy_node_init(void *priv)
  92 {
  93         struct sys_t_policy_node *pn = priv;
  94
  95         uuid_gen(&pn->uuid);
  96 }
  97
  98 static int sys_t_output_open(void *priv, struct stm_output *output)
  99 {
 100         struct sys_t_policy_node *pn = priv;
 101         struct sys_t_output *opriv;
 102
 103         opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);
 104         if (!opriv)
 105                 return -ENOMEM;
 106
 107         memcpy(&opriv->node, pn, sizeof(opriv->node));
 108         output->pdrv_private = opriv;
 109
 110         return 0;
 111 }
 112
 113 static void sys_t_output_close(struct stm_output *output)
 114 {
 115         kfree(output->pdrv_private);
 116 }
 117
 118 static ssize_t sys_t_policy_uuid_show(struct config_item *item,
 119                                       char *page)
 120 {
 121         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 122
 123         return sprintf(page, "%pU\n", &pn->uuid);
 124 }
 125
 126 static ssize_t
 127 sys_t_policy_uuid_store(struct config_item *item, const char *page,
 128                         size_t count)
 129 {
 130         struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
 131         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 132         int ret;
 133
 134         mutex_lock(mutexp);
 135         ret = uuid_parse(page, &pn->uuid);
 136         mutex_unlock(mutexp);
 137
 138         return ret < 0 ? ret : count;
 139 }
 140
 141 CONFIGFS_ATTR(sys_t_policy_, uuid);
 142
 143 static ssize_t sys_t_policy_do_len_show(struct config_item *item,
 144                                       char *page)
 145 {
 146         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 147
 148         return sprintf(page, "%d\n", pn->do_len);
 149 }
 150
 151 static ssize_t
 152 sys_t_policy_do_len_store(struct config_item *item, const char *page,
 153                         size_t count)
 154 {
 155         struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
 156         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 157         int ret;
 158
 159         mutex_lock(mutexp);
 160         ret = kstrtobool(page, &pn->do_len);
 161         mutex_unlock(mutexp);
 162
 163         return ret ? ret : count;
 164 }
 165
 166 CONFIGFS_ATTR(sys_t_policy_, do_len);
 167
 168 static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,
 169                                              char *page)
 170 {
 171         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 172
 173         return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));
 174 }
 175
 176 static ssize_t
 177 sys_t_policy_ts_interval_store(struct config_item *item, const char *page,
 178                                size_t count)
 179 {
 180         struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
 181         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 182         unsigned int ms;
 183         int ret;
 184
 185         mutex_lock(mutexp);
 186         ret = kstrtouint(page, 10, &ms);
 187         mutex_unlock(mutexp);
 188
 189         if (!ret) {
 190                 pn->ts_interval = msecs_to_jiffies(ms);
 191                 return count;
 192         }
 193
 194         return ret;
 195 }
 196
 197 CONFIGFS_ATTR(sys_t_policy_, ts_interval);
 198
 199 static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,
 200                                                     char *page)
 201 {
 202         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 203
 204         return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));
 205 }
 206
 207 static ssize_t
 208 sys_t_policy_clocksync_interval_store(struct config_item *item,
 209                                       const char *page, size_t count)
 210 {
 211         struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
 212         struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
 213         unsigned int ms;
 214         int ret;
 215
 216         mutex_lock(mutexp);
 217         ret = kstrtouint(page, 10, &ms);
 218         mutex_unlock(mutexp);
 219
 220         if (!ret) {
 221                 pn->clocksync_interval = msecs_to_jiffies(ms);
 222                 return count;
 223         }
 224
 225         return ret;
 226 }
 227
 228 CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);
 229
 230 static struct configfs_attribute *sys_t_policy_attrs[] = {
 231         &sys_t_policy_attr_uuid,
 232         &sys_t_policy_attr_do_len,
 233         &sys_t_policy_attr_ts_interval,
 234         &sys_t_policy_attr_clocksync_interval,
 235         NULL,
 236 };
 237
 238 static inline bool sys_t_need_ts(struct sys_t_output *op)
 239 {
 240         if (op->node.ts_interval &&
 241             time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
 242                 op->ts_jiffies = jiffies;
 243
 244                 return true;
 245         }
 246
 247         return false;
 248 }
 249
 250 static bool sys_t_need_clock_sync(struct sys_t_output *op)
 251 {
 252         if (op->node.clocksync_interval &&
 253             time_after(jiffies,
 254                        op->clocksync_jiffies + op->node.clocksync_interval)) {
 255                 op->clocksync_jiffies = jiffies;
 256
 257                 return true;
 258         }
 259
 260         return false;
 261 }
 262
 263 static ssize_t
 264 sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)
 265 {
 266         u32 header = CLOCK_SYNC_HEADER;
 267         const unsigned char nil = 0;
 268         u64 payload[2]; /* Clock value and frequency */
 269         ssize_t sz;
 270
 271         sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
 272                           4, (u8 *)&header);
 273         if (sz <= 0)
 274                 return sz;
 275
 276         payload[0] = ktime_get_real_ns();
 277         payload[1] = NSEC_PER_SEC;
 278         sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));
 279         if (sz <= 0)
 280                 return sz;
 281
 282         data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
 283
 284         return sizeof(header) + sizeof(payload);
 285 }
 286
 287 static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,
 288                            unsigned int chan, const char *buf, size_t count)
 289 {
 290         struct sys_t_output *op = output->pdrv_private;
 291         unsigned int c = output->channel + chan;
 292         unsigned int m = output->master;
 293         const unsigned char nil = 0;
 294         u32 header = DATA_HEADER;
 295         u8 uuid[UUID_SIZE];
 296         ssize_t sz;
 297
 298         /* We require an existing policy node to proceed */
 299         if (!op)
 300                 return -EINVAL;
 301
 302         if (sys_t_need_clock_sync(op)) {
 303                 sz = sys_t_clock_sync(data, m, c);
 304                 if (sz <= 0)
 305                         return sz;
 306         }
 307
 308         if (op->node.do_len)
 309                 header |= MIPI_SYST_OPT_LEN;
 310         if (sys_t_need_ts(op))
 311                 header |= MIPI_SYST_OPT_TS;
 312
 313         /*
 314          * STP framing rules for SyS-T frames:
 315          *   * the first packet of the SyS-T frame is timestamped;
 316          *   * the last packet is a FLAG.
 317          */
 318         /* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
 319         /* HEADER */
 320         sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
 321                           4, (u8 *)&header);
 322         if (sz <= 0)
 323                 return sz;
 324
 325         /* GUID */
 326         export_uuid(uuid, &op->node.uuid);
 327         sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid));
 328         if (sz <= 0)
 329                 return sz;
 330
 331         /* [LENGTH] */
 332         if (op->node.do_len) {
 333                 u16 length = count;
 334
 335                 sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
 336                                   (u8 *)&length);
 337                 if (sz <= 0)
 338                         return sz;
 339         }
 340
 341         /* [TIMESTAMP] */
 342         if (header & MIPI_SYST_OPT_TS) {
 343                 u64 ts = ktime_get_real_ns();
 344
 345                 sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
 346                 if (sz <= 0)
 347                         return sz;
 348         }
 349
 350         /* DATA */
 351         sz = stm_data_write(data, m, c, false, buf, count);
 352         if (sz > 0)
 353                 data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
 354
 355         return sz;
 356 }
 357
 358 static const struct stm_protocol_driver sys_t_pdrv = {
 359         .owner                  = THIS_MODULE,
 360         .name                   = "p_sys-t",
 361         .priv_sz                = sizeof(struct sys_t_policy_node),
 362         .write                  = sys_t_write,
 363         .policy_attr            = sys_t_policy_attrs,
 364         .policy_node_init       = sys_t_policy_node_init,
 365         .output_open            = sys_t_output_open,
 366         .output_close           = sys_t_output_close,
 367 };
 368
 369 static int sys_t_stm_init(void)
 370 {
 371         return stm_register_protocol(&sys_t_pdrv);
 372 }
 373
 374 static void sys_t_stm_exit(void)
 375 {
 376         stm_unregister_protocol(&sys_t_pdrv);
 377 }
 378
 379 module_init(sys_t_stm_init);
 380 module_exit(sys_t_stm_exit);
 381
 382 MODULE_LICENSE("GPL v2");
 383 MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
 384 MODULE_AUTHOR("Alexander Shishkin <[email protected]>");