diff --git a/drivers/net/wwan/wwan_hwsim.c b/drivers/net/wwan/wwan_hwsim.c index 11d15dc39041..8541bd58e831 100644 --- a/drivers/net/wwan/wwan_hwsim.c +++ b/drivers/net/wwan/wwan_hwsim.c @@ -2,7 +2,7 @@ /* * WWAN device simulator for WWAN framework testing. * - * Copyright (c) 2021, Sergey Ryazanov + * Copyright (c) 2021, 2025, Sergey Ryazanov */ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt @@ -12,8 +12,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -65,6 +67,9 @@ struct wwan_hwsim_port { AT_PARSER_SKIP_LINE, } pstate; } at_emul; + struct { + struct timer_list timer; + } nmea_emul; }; }; @@ -193,6 +198,108 @@ static const struct wwan_port_ops wwan_hwsim_at_emul_port_ops = { .tx = wwan_hwsim_at_emul_tx, }; +#if IS_ENABLED(CONFIG_GNSS) +#define NMEA_MAX_LEN 82 /* Max sentence length */ +#define NMEA_TRAIL_LEN 5 /* '*' + Checksum + */ +#define NMEA_MAX_DATA_LEN (NMEA_MAX_LEN - NMEA_TRAIL_LEN) + +static __printf(2, 3) +void wwan_hwsim_nmea_skb_push_sentence(struct sk_buff *skb, + const char *fmt, ...) +{ + unsigned char *s, *p; + va_list ap; + u8 cs = 0; + int len; + + s = skb_put(skb, NMEA_MAX_LEN + 1); /* +'\0' */ + if (!s) + return; + + va_start(ap, fmt); + len = vsnprintf(s, NMEA_MAX_DATA_LEN + 1, fmt, ap); + va_end(ap); + if (WARN_ON_ONCE(len > NMEA_MAX_DATA_LEN))/* No space for trailer */ + return; + + for (p = s + 1; *p != '\0'; ++p)/* Skip leading '$' or '!' */ + cs ^= *p; + p += snprintf(p, 5 + 1, "*%02X\r\n", cs); + + len = (p - s) - (NMEA_MAX_LEN + 1); /* exp. vs real length diff */ + skb->tail += len; /* Adjust tail to real length */ + skb->len += len; +} + +static void wwan_hwsim_nmea_emul_timer(struct timer_list *t) +{ + /* 43.74754722298909 N 11.25759835922875 E in DMM format */ + static const unsigned int coord[4 * 2] = { 43, 44, 8528, 0, + 11, 15, 4559, 0 }; + struct wwan_hwsim_port *port = timer_container_of(port, t, nmea_emul.timer); + struct sk_buff *skb; + struct tm tm; + + time64_to_tm(ktime_get_real_seconds(), 0, &tm); + + mod_timer(&port->nmea_emul.timer, jiffies + HZ); /* 1 second */ + + skb = alloc_skb(NMEA_MAX_LEN * 2 + 2, GFP_ATOMIC); /* GGA + RMC */ + if (!skb) + return; + + wwan_hwsim_nmea_skb_push_sentence(skb, + "$GPGGA,%02u%02u%02u.000,%02u%02u.%04u,%c,%03u%02u.%04u,%c,1,7,1.03,176.2,M,55.2,M,,", + tm.tm_hour, tm.tm_min, tm.tm_sec, + coord[0], coord[1], coord[2], + coord[3] ? 'S' : 'N', + coord[4], coord[5], coord[6], + coord[7] ? 'W' : 'E'); + + wwan_hwsim_nmea_skb_push_sentence(skb, + "$GPRMC,%02u%02u%02u.000,A,%02u%02u.%04u,%c,%03u%02u.%04u,%c,0.02,31.66,%02u%02u%02u,,,A", + tm.tm_hour, tm.tm_min, tm.tm_sec, + coord[0], coord[1], coord[2], + coord[3] ? 'S' : 'N', + coord[4], coord[5], coord[6], + coord[7] ? 'W' : 'E', + tm.tm_mday, tm.tm_mon + 1, + (unsigned int)tm.tm_year - 100); + + wwan_port_rx(port->wwan, skb); +} + +static int wwan_hwsim_nmea_emul_start(struct wwan_port *wport) +{ + struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport); + + timer_setup(&port->nmea_emul.timer, wwan_hwsim_nmea_emul_timer, 0); + wwan_hwsim_nmea_emul_timer(&port->nmea_emul.timer); + + return 0; +} + +static void wwan_hwsim_nmea_emul_stop(struct wwan_port *wport) +{ + struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport); + + timer_delete_sync(&port->nmea_emul.timer); +} + +static int wwan_hwsim_nmea_emul_tx(struct wwan_port *wport, struct sk_buff *in) +{ + consume_skb(in); + + return 0; +} + +static const struct wwan_port_ops wwan_hwsim_nmea_emul_port_ops = { + .start = wwan_hwsim_nmea_emul_start, + .stop = wwan_hwsim_nmea_emul_stop, + .tx = wwan_hwsim_nmea_emul_tx, +}; +#endif + static struct wwan_hwsim_port *wwan_hwsim_port_new(struct wwan_hwsim_dev *dev, enum wwan_port_type type) { @@ -203,6 +310,10 @@ static struct wwan_hwsim_port *wwan_hwsim_port_new(struct wwan_hwsim_dev *dev, if (type == WWAN_PORT_AT) ops = &wwan_hwsim_at_emul_port_ops; +#if IS_ENABLED(CONFIG_GNSS) + else if (type == WWAN_PORT_NMEA) + ops = &wwan_hwsim_nmea_emul_port_ops; +#endif else return ERR_PTR(-EINVAL); @@ -478,9 +589,10 @@ static int __init wwan_hwsim_init_devs(void) list_add_tail(&dev->list, &wwan_hwsim_devs); spin_unlock(&wwan_hwsim_devs_lock); - /* Create a couple of ports per each device to accelerate + /* Create a few various ports per each device to accelerate * the simulator readiness time. */ + for (j = 0; j < 2; ++j) { port = wwan_hwsim_port_new(dev, WWAN_PORT_AT); if (IS_ERR(port)) @@ -490,6 +602,18 @@ static int __init wwan_hwsim_init_devs(void) list_add_tail(&port->list, &dev->ports); spin_unlock(&dev->ports_lock); } + +#if IS_ENABLED(CONFIG_GNSS) + port = wwan_hwsim_port_new(dev, WWAN_PORT_NMEA); + if (IS_ERR(port)) { + dev_warn(&dev->dev, "failed to create initial NMEA port: %d\n", + (int)PTR_ERR(port)); + } else { + spin_lock(&dev->ports_lock); + list_add_tail(&port->list, &dev->ports); + spin_unlock(&dev->ports_lock); + } +#endif } return 0;