diff --git a/drivers/usb/gadget/function/u_audio.c b/drivers/usb/gadget/function/u_audio.c index f6358ccac18e..adde44613546 100644 --- a/drivers/usb/gadget/function/u_audio.c +++ b/drivers/usb/gadget/function/u_audio.c @@ -35,6 +35,8 @@ enum { UAC_RATE_CTRL, }; +#define CLK_PPM_GROUP_SIZE 20 + /* Runtime data params for one stream */ struct uac_rtd_params { struct snd_uac_chip *uac; /* parent chip */ @@ -1278,6 +1280,10 @@ static void g_audio_work(struct work_struct *data) snprintf(str, sizeof(str), "VOLUME=0x%hx", prm->volume); uac_event[2] = str; break; + case SET_AUDIO_CLK: + uac_event[0] = "USB_STATE=SET_AUDIO_CLK"; + snprintf(str, sizeof(str), "PPM=%d", audio->params.ppm); + uac_event[1] = str; default: break; } @@ -1290,6 +1296,107 @@ static void g_audio_work(struct work_struct *data) } } +static void ppm_calculate_work(struct work_struct *data) +{ + struct g_audio *g_audio = container_of(data, struct g_audio, + ppm_work.work); + struct usb_gadget *gadget = g_audio->gadget; + uint32_t frame_number, fn_msec, clk_msec; + struct frame_number_data *fn = g_audio->fn; + uint64_t time_now, time_msec_tmp; + int32_t ppm; + static int32_t ppms[CLK_PPM_GROUP_SIZE]; + static int32_t ppm_sum; + int32_t cnt = fn->second % CLK_PPM_GROUP_SIZE; + + time_now = ktime_get_raw(); + frame_number = gadget->ops->get_frame(gadget); + + if (g_audio->fn->time_last && + time_now - g_audio->fn->time_last > 1500000000ULL) + dev_warn(g_audio->device, "PPM work scheduled too slow!\n"); + + g_audio->fn->time_last = time_now; + + /* + * If usb is disconnected, the controller will not receive the + * SoF signal and frame number will be invalid. Because we can't + * get accurate time of disconnect and whether the gadget will be + * plugged into the same host next time or not. We must clear all + * statistics. + */ + if (gadget->state != USB_STATE_CONFIGURED) { + memset(g_audio->fn, 0, sizeof(*g_audio->fn)); + dev_dbg(g_audio->device, "Disconnect. frame number is cleared\n"); + goto out; + } + + /* Fist statistic to record begin frame number and system time */ + if (!g_audio->fn->second++) { + g_audio->fn->time_begin = g_audio->fn->time_last; + g_audio->fn->fn_begin = frame_number; + g_audio->fn->fn_last = frame_number; + goto out; + } + + /* + * For DWC3 Controller, only 13 bits is used to store frame(micro) + * number. In other words, the frame number will overflow at most + * 2.047 seconds. We add another registor fn_overflow the record + * total frame number. + */ + if (frame_number <= g_audio->fn->fn_last) + g_audio->fn->fn_overflow++; + g_audio->fn->fn_last = frame_number; + + if (!g_audio->fn->fn_overflow) + goto out; + + /* The lower 3 bits represent micro number frame, we don't need it */ + fn_msec = (((fn->fn_overflow - 1) << 14) + + (BIT(14) + fn->fn_last - fn->fn_begin) + BIT(2)) >> 3; + time_msec_tmp = fn->time_last - fn->time_begin + 500000ULL; + do_div(time_msec_tmp, 1000000U); + clk_msec = (uint32_t)time_msec_tmp; + + /* + * According to the definition of ppm: + * host_clk = (1 + ppm / 1000000) * gadget_clk + * we can get: + * ppm = (host_clk - gadget_clk) * 1000000 / gadget_clk + */ + ppm = (fn_msec > clk_msec) ? + (fn_msec - clk_msec) * 1000000L / clk_msec : + -((clk_msec - fn_msec) * 1000000L / clk_msec); + + ppm_sum = ppm_sum - ppms[cnt] + ppm; + ppms[cnt] = ppm; + + dev_dbg(g_audio->device, + "frame %u msec %u ppm_calc %d ppm_avage(%d) %d\n", + fn_msec, clk_msec, ppm, CLK_PPM_GROUP_SIZE, + ppm_sum / CLK_PPM_GROUP_SIZE); + + /* + * We calculate the average of ppm over a period of time. If the + * latest frame number is too far from the average, no event will + * be sent. + */ + if (abs(ppm_sum / CLK_PPM_GROUP_SIZE - ppm) < 3) { + ppm = ppm_sum > 0 ? + (ppm_sum + CLK_PPM_GROUP_SIZE / 2) / CLK_PPM_GROUP_SIZE : + (ppm_sum - CLK_PPM_GROUP_SIZE / 2) / CLK_PPM_GROUP_SIZE; + if (ppm != g_audio->params.ppm) { + g_audio->params.ppm = ppm; + g_audio->usb_state[SET_AUDIO_CLK] = true; + schedule_work(&g_audio->work); + } + } + +out: + schedule_delayed_work(&g_audio->ppm_work, 1 * HZ); +} + int g_audio_setup(struct g_audio *g_audio, const char *pcm_name, const char *card_name) { @@ -1314,6 +1421,12 @@ int g_audio_setup(struct g_audio *g_audio, const char *pcm_name, p_chmask = params->p_chmask; c_chmask = params->c_chmask; + g_audio->fn = kzalloc(sizeof(*g_audio->fn), GFP_KERNEL); + if (!g_audio->fn) { + err = -ENOMEM; + goto fail; + } + if (c_chmask) { struct uac_rtd_params *prm = &uac->c_prm; @@ -1543,6 +1656,8 @@ int g_audio_setup(struct g_audio *g_audio, const char *pcm_name, } INIT_WORK(&g_audio->work, g_audio_work); + INIT_DELAYED_WORK(&g_audio->ppm_work, ppm_calculate_work); + ppm_calculate_work(&g_audio->ppm_work.work); if (!err) return 0; @@ -1555,6 +1670,7 @@ fail: kfree(uac->p_prm.rbuf); kfree(uac->c_prm.rbuf); kfree(uac); + kfree(g_audio->fn); return err; } @@ -1569,6 +1685,7 @@ void g_audio_cleanup(struct g_audio *g_audio) return; cancel_work_sync(&g_audio->work); + cancel_delayed_work_sync(&g_audio->ppm_work); device_destroy(g_audio->device->class, g_audio->device->devt); g_audio->device = NULL; @@ -1582,6 +1699,7 @@ void g_audio_cleanup(struct g_audio *g_audio) kfree(uac->p_prm.rbuf); kfree(uac->c_prm.rbuf); kfree(uac); + kfree(g_audio->fn); } EXPORT_SYMBOL_GPL(g_audio_cleanup); diff --git a/drivers/usb/gadget/function/u_audio.h b/drivers/usb/gadget/function/u_audio.h index 2772ad9df5de..51286ed20daf 100644 --- a/drivers/usb/gadget/function/u_audio.h +++ b/drivers/usb/gadget/function/u_audio.h @@ -53,6 +53,8 @@ struct uac_params { /* rates are dynamic, in uac_rtd_params */ + int ppm; /* difference between audio clk and usb clk */ + int req_number; /* number of preallocated requests */ int fb_max; /* upper frequency drift feedback limit per-mil */ }; @@ -66,6 +68,7 @@ enum usb_state_index { SET_VOLUME_IN, SET_MUTE_OUT, SET_MUTE_IN, + SET_AUDIO_CLK, SET_USB_STATE_MAX, }; @@ -74,12 +77,24 @@ enum stream_state_index { STATE_IN, }; +struct frame_number_data { + uint32_t fn_begin; /* frame number when starting statistics */ + uint32_t fn_last; /* frame number in the latest statistics */ + uint32_t fn_overflow; /* the time of frame number overflow */ + uint32_t second; /* total seconds counted */ + ktime_t time_begin; /* system time when starting statistics */ + ktime_t time_last; /* system time in the latest statistics */ +}; + struct g_audio { struct device *device; bool usb_state[SET_USB_STATE_MAX]; bool stream_state[2]; struct work_struct work; + struct frame_number_data *fn; + struct delayed_work ppm_work; + struct usb_function func; struct usb_gadget *gadget;