2 * Intel(R) Trace Hub Software Trace Hub support
4 * Copyright (C) 2014-2015 Intel Corporation.
6 * This program is free software; you can redistribute it and/or modify it
7 * under the terms and conditions of the GNU General Public License,
8 * version 2, as published by the Free Software Foundation.
10 * This program is distributed in the hope it will be useful, but WITHOUT
11 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
12 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
16 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
18 #include <linux/types.h>
19 #include <linux/module.h>
20 #include <linux/device.h>
23 #include <linux/slab.h>
24 #include <linux/stm.h>
31 void __iomem
*channels
;
32 phys_addr_t channels_phys
;
35 unsigned int sw_nmasters
;
38 static struct intel_th_channel __iomem
*
39 sth_channel(struct sth_device
*sth
, unsigned int master
, unsigned int channel
)
41 struct intel_th_channel __iomem
*sw_map
= sth
->channels
;
43 return &sw_map
[(master
- sth
->stm
.sw_start
) * sth
->stm
.sw_nchannels
+
47 static void sth_iowrite(void __iomem
*dest
, const unsigned char *payload
,
53 writeq_relaxed(*(u64
*)payload
, dest
);
57 writel_relaxed(*(u32
*)payload
, dest
);
60 writew_relaxed(*(u16
*)payload
, dest
);
63 writeb_relaxed(*(u8
*)payload
, dest
);
70 static ssize_t
sth_stm_packet(struct stm_data
*stm_data
, unsigned int master
,
71 unsigned int channel
, unsigned int packet
,
72 unsigned int flags
, unsigned int size
,
73 const unsigned char *payload
)
75 struct sth_device
*sth
= container_of(stm_data
, struct sth_device
, stm
);
76 struct intel_th_channel __iomem
*out
=
77 sth_channel(sth
, master
, channel
);
78 u64 __iomem
*outp
= &out
->Dn
;
79 unsigned long reg
= REG_STH_TRIG
;
86 size
= rounddown_pow_of_two(size
);
89 /* Global packets (GERR, XSYNC, TRIG) are sent with register writes */
92 case STP_PACKET_XSYNC
:
95 if (flags
& STP_PACKET_TIMESTAMPED
)
97 iowrite8(*payload
, sth
->base
+ reg
);
100 case STP_PACKET_MERR
:
101 sth_iowrite(&out
->MERR
, payload
, size
);
104 case STP_PACKET_FLAG
:
105 if (flags
& STP_PACKET_TIMESTAMPED
)
106 outp
= (u64 __iomem
*)&out
->FLAG_TS
;
108 outp
= (u64 __iomem
*)&out
->FLAG
;
111 sth_iowrite(outp
, payload
, size
);
114 case STP_PACKET_USER
:
115 if (flags
& STP_PACKET_TIMESTAMPED
)
116 outp
= &out
->USER_TS
;
119 sth_iowrite(outp
, payload
, size
);
122 case STP_PACKET_DATA
:
125 if (flags
& STP_PACKET_TIMESTAMPED
)
127 if (flags
& STP_PACKET_MARKED
)
130 sth_iowrite(outp
, payload
, size
);
138 sth_stm_mmio_addr(struct stm_data
*stm_data
, unsigned int master
,
139 unsigned int channel
, unsigned int nr_chans
)
141 struct sth_device
*sth
= container_of(stm_data
, struct sth_device
, stm
);
144 master
-= sth
->stm
.sw_start
;
145 addr
= sth
->channels_phys
+ (master
* sth
->stm
.sw_nchannels
+ channel
) *
146 sizeof(struct intel_th_channel
);
148 if (offset_in_page(addr
) ||
149 offset_in_page(nr_chans
* sizeof(struct intel_th_channel
)))
155 static int sth_stm_link(struct stm_data
*stm_data
, unsigned int master
,
156 unsigned int channel
)
158 struct sth_device
*sth
= container_of(stm_data
, struct sth_device
, stm
);
160 intel_th_set_output(to_intel_th_device(sth
->dev
), master
);
165 static int intel_th_sw_init(struct sth_device
*sth
)
169 reg
= ioread32(sth
->base
+ REG_STH_STHCAP1
);
170 sth
->stm
.sw_nchannels
= reg
& 0xff;
172 reg
= ioread32(sth
->base
+ REG_STH_STHCAP0
);
173 sth
->stm
.sw_start
= reg
& 0xffff;
174 sth
->stm
.sw_end
= reg
>> 16;
176 sth
->sw_nmasters
= sth
->stm
.sw_end
- sth
->stm
.sw_start
;
177 dev_dbg(sth
->dev
, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n",
178 sth
->stm
.sw_start
, sth
->stm
.sw_end
, sth
->sw_nmasters
,
179 sth
->stm
.sw_nchannels
);
184 static int intel_th_sth_probe(struct intel_th_device
*thdev
)
186 struct device
*dev
= &thdev
->dev
;
187 struct sth_device
*sth
;
188 struct resource
*res
;
189 void __iomem
*base
, *channels
;
192 res
= intel_th_device_get_resource(thdev
, IORESOURCE_MEM
, 0);
196 base
= devm_ioremap(dev
, res
->start
, resource_size(res
));
200 res
= intel_th_device_get_resource(thdev
, IORESOURCE_MEM
, 1);
204 channels
= devm_ioremap(dev
, res
->start
, resource_size(res
));
208 sth
= devm_kzalloc(dev
, sizeof(*sth
), GFP_KERNEL
);
214 sth
->channels
= channels
;
215 sth
->channels_phys
= res
->start
;
216 sth
->stm
.name
= dev_name(dev
);
217 sth
->stm
.packet
= sth_stm_packet
;
218 sth
->stm
.mmio_addr
= sth_stm_mmio_addr
;
219 sth
->stm
.sw_mmiosz
= sizeof(struct intel_th_channel
);
220 sth
->stm
.link
= sth_stm_link
;
222 err
= intel_th_sw_init(sth
);
226 err
= stm_register_device(dev
, &sth
->stm
, THIS_MODULE
);
228 dev_err(dev
, "stm_register_device failed\n");
232 dev_set_drvdata(dev
, sth
);
237 static void intel_th_sth_remove(struct intel_th_device
*thdev
)
239 struct sth_device
*sth
= dev_get_drvdata(&thdev
->dev
);
241 stm_unregister_device(&sth
->stm
);
244 static struct intel_th_driver intel_th_sth_driver
= {
245 .probe
= intel_th_sth_probe
,
246 .remove
= intel_th_sth_remove
,
249 .owner
= THIS_MODULE
,
253 module_driver(intel_th_sth_driver
,
254 intel_th_driver_register
,
255 intel_th_driver_unregister
);
257 MODULE_LICENSE("GPL v2");
258 MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver");
259 MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>");