hwtracing/stm/p_sys-t.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * MIPI SyS-T framing protocol for STM devices.
0004  * Copyright (c) 2018, Intel Corporation.
0005  */
0006
0007 #include <linux/configfs.h>
0008 #include <linux/module.h>
0009 #include <linux/device.h>
0010 #include <linux/slab.h>
0011 #include <linux/uuid.h>
0012 #include <linux/stm.h>
0013 #include "stm.h"
0014
0015 enum sys_t_message_type {
0016     MIPI_SYST_TYPE_BUILD    = 0,
0017     MIPI_SYST_TYPE_SHORT32,
0018     MIPI_SYST_TYPE_STRING,
0019     MIPI_SYST_TYPE_CATALOG,
0020     MIPI_SYST_TYPE_RAW  = 6,
0021     MIPI_SYST_TYPE_SHORT64,
0022     MIPI_SYST_TYPE_CLOCK,
0023 };
0024
0025 enum sys_t_message_severity {
0026     MIPI_SYST_SEVERITY_MAX  = 0,
0027     MIPI_SYST_SEVERITY_FATAL,
0028     MIPI_SYST_SEVERITY_ERROR,
0029     MIPI_SYST_SEVERITY_WARNING,
0030     MIPI_SYST_SEVERITY_INFO,
0031     MIPI_SYST_SEVERITY_USER1,
0032     MIPI_SYST_SEVERITY_USER2,
0033     MIPI_SYST_SEVERITY_DEBUG,
0034 };
0035
0036 enum sys_t_message_build_subtype {
0037     MIPI_SYST_BUILD_ID_COMPACT32 = 0,
0038     MIPI_SYST_BUILD_ID_COMPACT64,
0039     MIPI_SYST_BUILD_ID_LONG,
0040 };
0041
0042 enum sys_t_message_clock_subtype {
0043     MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
0044 };
0045
0046 enum sys_t_message_string_subtype {
0047     MIPI_SYST_STRING_GENERIC    = 1,
0048     MIPI_SYST_STRING_FUNCTIONENTER,
0049     MIPI_SYST_STRING_FUNCTIONEXIT,
0050     MIPI_SYST_STRING_INVALIDPARAM   = 5,
0051     MIPI_SYST_STRING_ASSERT     = 7,
0052     MIPI_SYST_STRING_PRINTF_32  = 11,
0053     MIPI_SYST_STRING_PRINTF_64  = 12,
0054 };
0055
0056 #define MIPI_SYST_TYPE(t)       ((u32)(MIPI_SYST_TYPE_ ## t))
0057 #define MIPI_SYST_SEVERITY(s)       ((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)
0058 #define MIPI_SYST_OPT_LOC       BIT(8)
0059 #define MIPI_SYST_OPT_LEN       BIT(9)
0060 #define MIPI_SYST_OPT_CHK       BIT(10)
0061 #define MIPI_SYST_OPT_TS        BIT(11)
0062 #define MIPI_SYST_UNIT(u)       ((u32)(u) << 12)
0063 #define MIPI_SYST_ORIGIN(o)     ((u32)(o) << 16)
0064 #define MIPI_SYST_OPT_GUID      BIT(23)
0065 #define MIPI_SYST_SUBTYPE(s)        ((u32)(MIPI_SYST_ ## s) << 24)
0066 #define MIPI_SYST_UNITLARGE(u)      (MIPI_SYST_UNIT(u & 0xf) | \
0067                      MIPI_SYST_ORIGIN(u >> 4))
0068 #define MIPI_SYST_TYPES(t, s)       (MIPI_SYST_TYPE(t) | \
0069                      MIPI_SYST_SUBTYPE(t ## _ ## s))
0070
0071 #define DATA_HEADER (MIPI_SYST_TYPES(STRING, GENERIC)   | \
0072              MIPI_SYST_SEVERITY(INFO)       | \
0073              MIPI_SYST_OPT_GUID)
0074
0075 #define CLOCK_SYNC_HEADER   (MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC) | \
0076                  MIPI_SYST_SEVERITY(MAX))
0077
0078 struct sys_t_policy_node {
0079     uuid_t      uuid;
0080     bool        do_len;
0081     unsigned long   ts_interval;
0082     unsigned long   clocksync_interval;
0083 };
0084
0085 struct sys_t_output {
0086     struct sys_t_policy_node    node;
0087     unsigned long   ts_jiffies;
0088     unsigned long   clocksync_jiffies;
0089 };
0090
0091 static void sys_t_policy_node_init(void *priv)
0092 {
0093     struct sys_t_policy_node *pn = priv;
0094
0095     uuid_gen(&pn->uuid);
0096 }
0097
0098 static int sys_t_output_open(void *priv, struct stm_output *output)
0099 {
0100     struct sys_t_policy_node *pn = priv;
0101     struct sys_t_output *opriv;
0102
0103     opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);
0104     if (!opriv)
0105         return -ENOMEM;
0106
0107     memcpy(&opriv->node, pn, sizeof(opriv->node));
0108     output->pdrv_private = opriv;
0109
0110     return 0;
0111 }
0112
0113 static void sys_t_output_close(struct stm_output *output)
0114 {
0115     kfree(output->pdrv_private);
0116 }
0117
0118 static ssize_t sys_t_policy_uuid_show(struct config_item *item,
0119                       char *page)
0120 {
0121     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0122
0123     return sprintf(page, "%pU\n", &pn->uuid);
0124 }
0125
0126 static ssize_t
0127 sys_t_policy_uuid_store(struct config_item *item, const char *page,
0128             size_t count)
0129 {
0130     struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
0131     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0132     int ret;
0133
0134     mutex_lock(mutexp);
0135     ret = uuid_parse(page, &pn->uuid);
0136     mutex_unlock(mutexp);
0137
0138     return ret < 0 ? ret : count;
0139 }
0140
0141 CONFIGFS_ATTR(sys_t_policy_, uuid);
0142
0143 static ssize_t sys_t_policy_do_len_show(struct config_item *item,
0144                       char *page)
0145 {
0146     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0147
0148     return sprintf(page, "%d\n", pn->do_len);
0149 }
0150
0151 static ssize_t
0152 sys_t_policy_do_len_store(struct config_item *item, const char *page,
0153             size_t count)
0154 {
0155     struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
0156     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0157     int ret;
0158
0159     mutex_lock(mutexp);
0160     ret = kstrtobool(page, &pn->do_len);
0161     mutex_unlock(mutexp);
0162
0163     return ret ? ret : count;
0164 }
0165
0166 CONFIGFS_ATTR(sys_t_policy_, do_len);
0167
0168 static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,
0169                          char *page)
0170 {
0171     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0172
0173     return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));
0174 }
0175
0176 static ssize_t
0177 sys_t_policy_ts_interval_store(struct config_item *item, const char *page,
0178                    size_t count)
0179 {
0180     struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
0181     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0182     unsigned int ms;
0183     int ret;
0184
0185     mutex_lock(mutexp);
0186     ret = kstrtouint(page, 10, &ms);
0187     mutex_unlock(mutexp);
0188
0189     if (!ret) {
0190         pn->ts_interval = msecs_to_jiffies(ms);
0191         return count;
0192     }
0193
0194     return ret;
0195 }
0196
0197 CONFIGFS_ATTR(sys_t_policy_, ts_interval);
0198
0199 static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,
0200                             char *page)
0201 {
0202     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0203
0204     return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));
0205 }
0206
0207 static ssize_t
0208 sys_t_policy_clocksync_interval_store(struct config_item *item,
0209                       const char *page, size_t count)
0210 {
0211     struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
0212     struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
0213     unsigned int ms;
0214     int ret;
0215
0216     mutex_lock(mutexp);
0217     ret = kstrtouint(page, 10, &ms);
0218     mutex_unlock(mutexp);
0219
0220     if (!ret) {
0221         pn->clocksync_interval = msecs_to_jiffies(ms);
0222         return count;
0223     }
0224
0225     return ret;
0226 }
0227
0228 CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);
0229
0230 static struct configfs_attribute *sys_t_policy_attrs[] = {
0231     &sys_t_policy_attr_uuid,
0232     &sys_t_policy_attr_do_len,
0233     &sys_t_policy_attr_ts_interval,
0234     &sys_t_policy_attr_clocksync_interval,
0235     NULL,
0236 };
0237
0238 static inline bool sys_t_need_ts(struct sys_t_output *op)
0239 {
0240     if (op->node.ts_interval &&
0241         time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
0242         op->ts_jiffies = jiffies;
0243
0244         return true;
0245     }
0246
0247     return false;
0248 }
0249
0250 static bool sys_t_need_clock_sync(struct sys_t_output *op)
0251 {
0252     if (op->node.clocksync_interval &&
0253         time_after(jiffies,
0254                op->clocksync_jiffies + op->node.clocksync_interval)) {
0255         op->clocksync_jiffies = jiffies;
0256
0257         return true;
0258     }
0259
0260     return false;
0261 }
0262
0263 static ssize_t
0264 sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)
0265 {
0266     u32 header = CLOCK_SYNC_HEADER;
0267     const unsigned char nil = 0;
0268     u64 payload[2]; /* Clock value and frequency */
0269     ssize_t sz;
0270
0271     sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
0272               4, (u8 *)&header);
0273     if (sz <= 0)
0274         return sz;
0275
0276     payload[0] = ktime_get_real_ns();
0277     payload[1] = NSEC_PER_SEC;
0278     sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));
0279     if (sz <= 0)
0280         return sz;
0281
0282     data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
0283
0284     return sizeof(header) + sizeof(payload);
0285 }
0286
0287 static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,
0288                unsigned int chan, const char *buf, size_t count)
0289 {
0290     struct sys_t_output *op = output->pdrv_private;
0291     unsigned int c = output->channel + chan;
0292     unsigned int m = output->master;
0293     const unsigned char nil = 0;
0294     u32 header = DATA_HEADER;
0295     u8 uuid[UUID_SIZE];
0296     ssize_t sz;
0297
0298     /* We require an existing policy node to proceed */
0299     if (!op)
0300         return -EINVAL;
0301
0302     if (sys_t_need_clock_sync(op)) {
0303         sz = sys_t_clock_sync(data, m, c);
0304         if (sz <= 0)
0305             return sz;
0306     }
0307
0308     if (op->node.do_len)
0309         header |= MIPI_SYST_OPT_LEN;
0310     if (sys_t_need_ts(op))
0311         header |= MIPI_SYST_OPT_TS;
0312
0313     /*
0314      * STP framing rules for SyS-T frames:
0315      *   * the first packet of the SyS-T frame is timestamped;
0316      *   * the last packet is a FLAG.
0317      */
0318     /* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
0319     /* HEADER */
0320     sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
0321               4, (u8 *)&header);
0322     if (sz <= 0)
0323         return sz;
0324
0325     /* GUID */
0326     export_uuid(uuid, &op->node.uuid);
0327     sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid));
0328     if (sz <= 0)
0329         return sz;
0330
0331     /* [LENGTH] */
0332     if (op->node.do_len) {
0333         u16 length = count;
0334
0335         sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
0336                   (u8 *)&length);
0337         if (sz <= 0)
0338             return sz;
0339     }
0340
0341     /* [TIMESTAMP] */
0342     if (header & MIPI_SYST_OPT_TS) {
0343         u64 ts = ktime_get_real_ns();
0344
0345         sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
0346         if (sz <= 0)
0347             return sz;
0348     }
0349
0350     /* DATA */
0351     sz = stm_data_write(data, m, c, false, buf, count);
0352     if (sz > 0)
0353         data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
0354
0355     return sz;
0356 }
0357
0358 static const struct stm_protocol_driver sys_t_pdrv = {
0359     .owner          = THIS_MODULE,
0360     .name           = "p_sys-t",
0361     .priv_sz        = sizeof(struct sys_t_policy_node),
0362     .write          = sys_t_write,
0363     .policy_attr        = sys_t_policy_attrs,
0364     .policy_node_init   = sys_t_policy_node_init,
0365     .output_open        = sys_t_output_open,
0366     .output_close       = sys_t_output_close,
0367 };
0368
0369 static int sys_t_stm_init(void)
0370 {
0371     return stm_register_protocol(&sys_t_pdrv);
0372 }
0373
0374 static void sys_t_stm_exit(void)
0375 {
0376     stm_unregister_protocol(&sys_t_pdrv);
0377 }
0378
0379 module_init(sys_t_stm_init);
0380 module_exit(sys_t_stm_exit);
0381
0382 MODULE_LICENSE("GPL v2");
0383 MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
0384 MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");