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 sys_t_policy_node_init(void * priv)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 sys_t_output_open(void * priv,struct stm_output * output)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 sys_t_output_close(struct stm_output * output)113 static void sys_t_output_close(struct stm_output *output) 114 { 115 kfree(output->pdrv_private); 116 } 117 sys_t_policy_uuid_show(struct config_item * item,char * page)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 sys_t_policy_uuid_store(struct config_item * item,const char * page,size_t count)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 sys_t_policy_do_len_show(struct config_item * item,char * page)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 sys_t_policy_do_len_store(struct config_item * item,const char * page,size_t count)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 sys_t_policy_ts_interval_show(struct config_item * item,char * page)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 sys_t_policy_ts_interval_store(struct config_item * item,const char * page,size_t count)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 sys_t_policy_clocksync_interval_show(struct config_item * item,char * page)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 sys_t_policy_clocksync_interval_store(struct config_item * item,const char * page,size_t count)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 sys_t_need_ts(struct sys_t_output * op)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 sys_t_need_clock_sync(struct sys_t_output * op)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 sys_t_clock_sync(struct stm_data * data,unsigned int m,unsigned int c)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 sys_t_write(struct stm_data * data,struct stm_output * output,unsigned int chan,const char * buf,size_t count)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 sys_t_stm_init(void)369 static int sys_t_stm_init(void) 370 { 371 return stm_register_protocol(&sys_t_pdrv); 372 } 373 sys_t_stm_exit(void)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 <alexander.shishkin@linux.intel.com>"); 385